Annotation of qemu/hw/fdc.c, revision 1.1.1.4

1.1       root        1: /*
                      2:  * QEMU Floppy disk emulator (Intel 82078)
1.1.1.3   root        3:  *
                      4:  * Copyright (c) 2003, 2007 Jocelyn Mayer
1.1.1.4 ! root        5:  * Copyright (c) 2008 Hervé Poussineau
1.1.1.3   root        6:  *
1.1       root        7:  * Permission is hereby granted, free of charge, to any person obtaining a copy
                      8:  * of this software and associated documentation files (the "Software"), to deal
                      9:  * in the Software without restriction, including without limitation the rights
                     10:  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
                     11:  * copies of the Software, and to permit persons to whom the Software is
                     12:  * furnished to do so, subject to the following conditions:
                     13:  *
                     14:  * The above copyright notice and this permission notice shall be included in
                     15:  * all copies or substantial portions of the Software.
                     16:  *
                     17:  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
                     18:  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
                     19:  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
                     20:  * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
                     21:  * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
                     22:  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
                     23:  * THE SOFTWARE.
                     24:  */
                     25: /*
                     26:  * The controller is used in Sun4m systems in a slightly different
                     27:  * way. There are changes in DOR register and DMA is not available.
                     28:  */
1.1.1.3   root       29: #include "hw.h"
                     30: #include "fdc.h"
                     31: #include "block.h"
                     32: #include "qemu-timer.h"
                     33: #include "isa.h"
1.1       root       34: 
                     35: /********************************************************/
                     36: /* debug Floppy devices */
                     37: //#define DEBUG_FLOPPY
                     38: 
                     39: #ifdef DEBUG_FLOPPY
                     40: #define FLOPPY_DPRINTF(fmt, args...) \
                     41: do { printf("FLOPPY: " fmt , ##args); } while (0)
                     42: #else
                     43: #define FLOPPY_DPRINTF(fmt, args...)
                     44: #endif
                     45: 
                     46: #define FLOPPY_ERROR(fmt, args...) \
                     47: do { printf("FLOPPY ERROR: %s: " fmt, __func__ , ##args); } while (0)
                     48: 
                     49: /********************************************************/
                     50: /* Floppy drive emulation                               */
                     51: 
1.1.1.4 ! root       52: #define GET_CUR_DRV(fdctrl) ((fdctrl)->cur_drv)
        !            53: #define SET_CUR_DRV(fdctrl, drive) ((fdctrl)->cur_drv = (drive))
        !            54: 
1.1       root       55: /* Will always be a fixed parameter for us */
1.1.1.4 ! root       56: #define FD_SECTOR_LEN          512
        !            57: #define FD_SECTOR_SC           2   /* Sector size code */
        !            58: #define FD_RESET_SENSEI_COUNT  4   /* Number of sense interrupts on RESET */
1.1       root       59: 
                     60: /* Floppy disk drive emulation */
                     61: typedef enum fdisk_type_t {
                     62:     FDRIVE_DISK_288   = 0x01, /* 2.88 MB disk           */
                     63:     FDRIVE_DISK_144   = 0x02, /* 1.44 MB disk           */
                     64:     FDRIVE_DISK_720   = 0x03, /* 720 kB disk            */
                     65:     FDRIVE_DISK_USER  = 0x04, /* User defined geometry  */
                     66:     FDRIVE_DISK_NONE  = 0x05, /* No disk                */
                     67: } fdisk_type_t;
                     68: 
                     69: typedef enum fdrive_type_t {
                     70:     FDRIVE_DRV_144  = 0x00,   /* 1.44 MB 3"5 drive      */
                     71:     FDRIVE_DRV_288  = 0x01,   /* 2.88 MB 3"5 drive      */
                     72:     FDRIVE_DRV_120  = 0x02,   /* 1.2  MB 5"25 drive     */
                     73:     FDRIVE_DRV_NONE = 0x03,   /* No drive connected     */
                     74: } fdrive_type_t;
                     75: 
                     76: typedef enum fdisk_flags_t {
                     77:     FDISK_DBL_SIDES  = 0x01,
                     78: } fdisk_flags_t;
                     79: 
                     80: typedef struct fdrive_t {
                     81:     BlockDriverState *bs;
                     82:     /* Drive status */
                     83:     fdrive_type_t drive;
                     84:     uint8_t perpendicular;    /* 2.88 MB access mode    */
                     85:     /* Position */
                     86:     uint8_t head;
                     87:     uint8_t track;
                     88:     uint8_t sect;
                     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: static void fd_init (fdrive_t *drv, BlockDriverState *bs)
                     98: {
                     99:     /* Drive */
                    100:     drv->bs = bs;
                    101:     drv->drive = FDRIVE_DRV_NONE;
                    102:     drv->perpendicular = 0;
                    103:     /* Disk */
                    104:     drv->last_sect = 0;
                    105:     drv->max_track = 0;
                    106: }
                    107: 
                    108: static int _fd_sector (uint8_t head, uint8_t track,
1.1.1.3   root      109:                        uint8_t sect, uint8_t last_sect)
1.1       root      110: {
                    111:     return (((track * 2) + head) * last_sect) + sect - 1;
                    112: }
                    113: 
                    114: /* Returns current position, in sectors, for given drive */
                    115: static int fd_sector (fdrive_t *drv)
                    116: {
                    117:     return _fd_sector(drv->head, drv->track, drv->sect, drv->last_sect);
                    118: }
                    119: 
1.1.1.4 ! root      120: /* Seek to a new position:
        !           121:  * returns 0 if already on right track
        !           122:  * returns 1 if track changed
        !           123:  * returns 2 if track is invalid
        !           124:  * returns 3 if sector is invalid
        !           125:  * returns 4 if seek is disabled
        !           126:  */
1.1       root      127: static int fd_seek (fdrive_t *drv, uint8_t head, uint8_t track, uint8_t sect,
                    128:                     int enable_seek)
                    129: {
                    130:     uint32_t sector;
                    131:     int ret;
                    132: 
                    133:     if (track > drv->max_track ||
1.1.1.3   root      134:         (head != 0 && (drv->flags & FDISK_DBL_SIDES) == 0)) {
1.1       root      135:         FLOPPY_DPRINTF("try to read %d %02x %02x (max=%d %d %02x %02x)\n",
                    136:                        head, track, sect, 1,
                    137:                        (drv->flags & FDISK_DBL_SIDES) == 0 ? 0 : 1,
                    138:                        drv->max_track, drv->last_sect);
                    139:         return 2;
                    140:     }
                    141:     if (sect > drv->last_sect) {
                    142:         FLOPPY_DPRINTF("try to read %d %02x %02x (max=%d %d %02x %02x)\n",
                    143:                        head, track, sect, 1,
                    144:                        (drv->flags & FDISK_DBL_SIDES) == 0 ? 0 : 1,
                    145:                        drv->max_track, drv->last_sect);
                    146:         return 3;
                    147:     }
                    148:     sector = _fd_sector(head, track, sect, drv->last_sect);
                    149:     ret = 0;
                    150:     if (sector != fd_sector(drv)) {
                    151: #if 0
                    152:         if (!enable_seek) {
                    153:             FLOPPY_ERROR("no implicit seek %d %02x %02x (max=%d %02x %02x)\n",
                    154:                          head, track, sect, 1, drv->max_track, drv->last_sect);
                    155:             return 4;
                    156:         }
                    157: #endif
                    158:         drv->head = head;
1.1.1.3   root      159:         if (drv->track != track)
                    160:             ret = 1;
1.1       root      161:         drv->track = track;
                    162:         drv->sect = sect;
                    163:     }
                    164: 
                    165:     return ret;
                    166: }
                    167: 
                    168: /* Set drive back to track 0 */
                    169: static void fd_recalibrate (fdrive_t *drv)
                    170: {
                    171:     FLOPPY_DPRINTF("recalibrate\n");
                    172:     drv->head = 0;
                    173:     drv->track = 0;
                    174:     drv->sect = 1;
                    175: }
                    176: 
                    177: /* Recognize floppy formats */
                    178: typedef struct fd_format_t {
                    179:     fdrive_type_t drive;
                    180:     fdisk_type_t  disk;
                    181:     uint8_t last_sect;
                    182:     uint8_t max_track;
                    183:     uint8_t max_head;
1.1.1.3   root      184:     const char *str;
1.1       root      185: } fd_format_t;
                    186: 
1.1.1.3   root      187: static const fd_format_t fd_formats[] = {
1.1       root      188:     /* First entry is default format */
                    189:     /* 1.44 MB 3"1/2 floppy disks */
                    190:     { FDRIVE_DRV_144, FDRIVE_DISK_144, 18, 80, 1, "1.44 MB 3\"1/2", },
                    191:     { FDRIVE_DRV_144, FDRIVE_DISK_144, 20, 80, 1,  "1.6 MB 3\"1/2", },
                    192:     { FDRIVE_DRV_144, FDRIVE_DISK_144, 21, 80, 1, "1.68 MB 3\"1/2", },
                    193:     { FDRIVE_DRV_144, FDRIVE_DISK_144, 21, 82, 1, "1.72 MB 3\"1/2", },
                    194:     { FDRIVE_DRV_144, FDRIVE_DISK_144, 21, 83, 1, "1.74 MB 3\"1/2", },
                    195:     { FDRIVE_DRV_144, FDRIVE_DISK_144, 22, 80, 1, "1.76 MB 3\"1/2", },
                    196:     { FDRIVE_DRV_144, FDRIVE_DISK_144, 23, 80, 1, "1.84 MB 3\"1/2", },
                    197:     { FDRIVE_DRV_144, FDRIVE_DISK_144, 24, 80, 1, "1.92 MB 3\"1/2", },
                    198:     /* 2.88 MB 3"1/2 floppy disks */
                    199:     { FDRIVE_DRV_288, FDRIVE_DISK_288, 36, 80, 1, "2.88 MB 3\"1/2", },
                    200:     { FDRIVE_DRV_288, FDRIVE_DISK_288, 39, 80, 1, "3.12 MB 3\"1/2", },
                    201:     { FDRIVE_DRV_288, FDRIVE_DISK_288, 40, 80, 1,  "3.2 MB 3\"1/2", },
                    202:     { FDRIVE_DRV_288, FDRIVE_DISK_288, 44, 80, 1, "3.52 MB 3\"1/2", },
                    203:     { FDRIVE_DRV_288, FDRIVE_DISK_288, 48, 80, 1, "3.84 MB 3\"1/2", },
                    204:     /* 720 kB 3"1/2 floppy disks */
                    205:     { FDRIVE_DRV_144, FDRIVE_DISK_720,  9, 80, 1,  "720 kB 3\"1/2", },
                    206:     { FDRIVE_DRV_144, FDRIVE_DISK_720, 10, 80, 1,  "800 kB 3\"1/2", },
                    207:     { FDRIVE_DRV_144, FDRIVE_DISK_720, 10, 82, 1,  "820 kB 3\"1/2", },
                    208:     { FDRIVE_DRV_144, FDRIVE_DISK_720, 10, 83, 1,  "830 kB 3\"1/2", },
                    209:     { FDRIVE_DRV_144, FDRIVE_DISK_720, 13, 80, 1, "1.04 MB 3\"1/2", },
                    210:     { FDRIVE_DRV_144, FDRIVE_DISK_720, 14, 80, 1, "1.12 MB 3\"1/2", },
                    211:     /* 1.2 MB 5"1/4 floppy disks */
                    212:     { FDRIVE_DRV_120, FDRIVE_DISK_288, 15, 80, 1,  "1.2 kB 5\"1/4", },
                    213:     { FDRIVE_DRV_120, FDRIVE_DISK_288, 18, 80, 1, "1.44 MB 5\"1/4", },
                    214:     { FDRIVE_DRV_120, FDRIVE_DISK_288, 18, 82, 1, "1.48 MB 5\"1/4", },
                    215:     { FDRIVE_DRV_120, FDRIVE_DISK_288, 18, 83, 1, "1.49 MB 5\"1/4", },
                    216:     { FDRIVE_DRV_120, FDRIVE_DISK_288, 20, 80, 1,  "1.6 MB 5\"1/4", },
                    217:     /* 720 kB 5"1/4 floppy disks */
                    218:     { FDRIVE_DRV_120, FDRIVE_DISK_288,  9, 80, 1,  "720 kB 5\"1/4", },
                    219:     { FDRIVE_DRV_120, FDRIVE_DISK_288, 11, 80, 1,  "880 kB 5\"1/4", },
                    220:     /* 360 kB 5"1/4 floppy disks */
                    221:     { FDRIVE_DRV_120, FDRIVE_DISK_288,  9, 40, 1,  "360 kB 5\"1/4", },
                    222:     { FDRIVE_DRV_120, FDRIVE_DISK_288,  9, 40, 0,  "180 kB 5\"1/4", },
                    223:     { FDRIVE_DRV_120, FDRIVE_DISK_288, 10, 41, 1,  "410 kB 5\"1/4", },
                    224:     { FDRIVE_DRV_120, FDRIVE_DISK_288, 10, 42, 1,  "420 kB 5\"1/4", },
1.1.1.3   root      225:     /* 320 kB 5"1/4 floppy disks */
1.1       root      226:     { FDRIVE_DRV_120, FDRIVE_DISK_288,  8, 40, 1,  "320 kB 5\"1/4", },
                    227:     { FDRIVE_DRV_120, FDRIVE_DISK_288,  8, 40, 0,  "160 kB 5\"1/4", },
                    228:     /* 360 kB must match 5"1/4 better than 3"1/2... */
                    229:     { FDRIVE_DRV_144, FDRIVE_DISK_720,  9, 80, 0,  "360 kB 3\"1/2", },
                    230:     /* end */
                    231:     { FDRIVE_DRV_NONE, FDRIVE_DISK_NONE, -1, -1, 0, NULL, },
                    232: };
                    233: 
                    234: /* Revalidate a disk drive after a disk change */
                    235: static void fd_revalidate (fdrive_t *drv)
                    236: {
1.1.1.3   root      237:     const fd_format_t *parse;
                    238:     uint64_t nb_sectors, size;
1.1       root      239:     int i, first_match, match;
                    240:     int nb_heads, max_track, last_sect, ro;
                    241: 
                    242:     FLOPPY_DPRINTF("revalidate\n");
                    243:     if (drv->bs != NULL && bdrv_is_inserted(drv->bs)) {
1.1.1.3   root      244:         ro = bdrv_is_read_only(drv->bs);
                    245:         bdrv_get_geometry_hint(drv->bs, &nb_heads, &max_track, &last_sect);
                    246:         if (nb_heads != 0 && max_track != 0 && last_sect != 0) {
                    247:             FLOPPY_DPRINTF("User defined disk (%d %d %d)",
1.1       root      248:                            nb_heads - 1, max_track, last_sect);
1.1.1.3   root      249:         } else {
                    250:             bdrv_get_geometry(drv->bs, &nb_sectors);
                    251:             match = -1;
                    252:             first_match = -1;
                    253:             for (i = 0;; i++) {
                    254:                 parse = &fd_formats[i];
                    255:                 if (parse->drive == FDRIVE_DRV_NONE)
                    256:                     break;
                    257:                 if (drv->drive == parse->drive ||
                    258:                     drv->drive == FDRIVE_DRV_NONE) {
                    259:                     size = (parse->max_head + 1) * parse->max_track *
                    260:                         parse->last_sect;
                    261:                     if (nb_sectors == size) {
                    262:                         match = i;
                    263:                         break;
                    264:                     }
                    265:                     if (first_match == -1)
                    266:                         first_match = i;
                    267:                 }
                    268:             }
                    269:             if (match == -1) {
                    270:                 if (first_match == -1)
                    271:                     match = 1;
                    272:                 else
                    273:                     match = first_match;
                    274:                 parse = &fd_formats[match];
                    275:             }
                    276:             nb_heads = parse->max_head + 1;
                    277:             max_track = parse->max_track;
                    278:             last_sect = parse->last_sect;
                    279:             drv->drive = parse->drive;
                    280:             FLOPPY_DPRINTF("%s floppy disk (%d h %d t %d s) %s\n", parse->str,
1.1       root      281:                            nb_heads, max_track, last_sect, ro ? "ro" : "rw");
1.1.1.3   root      282:         }
                    283:         if (nb_heads == 1) {
                    284:             drv->flags &= ~FDISK_DBL_SIDES;
                    285:         } else {
                    286:             drv->flags |= FDISK_DBL_SIDES;
                    287:         }
                    288:         drv->max_track = max_track;
                    289:         drv->last_sect = last_sect;
                    290:         drv->ro = ro;
1.1       root      291:     } else {
1.1.1.3   root      292:         FLOPPY_DPRINTF("No disk in drive\n");
1.1       root      293:         drv->last_sect = 0;
1.1.1.3   root      294:         drv->max_track = 0;
                    295:         drv->flags &= ~FDISK_DBL_SIDES;
1.1       root      296:     }
                    297: }
                    298: 
                    299: /********************************************************/
                    300: /* Intel 82078 floppy disk controller emulation          */
                    301: 
                    302: static void fdctrl_reset (fdctrl_t *fdctrl, int do_irq);
                    303: static void fdctrl_reset_fifo (fdctrl_t *fdctrl);
                    304: static int fdctrl_transfer_handler (void *opaque, int nchan,
                    305:                                     int dma_pos, int dma_len);
1.1.1.4 ! root      306: static void fdctrl_raise_irq (fdctrl_t *fdctrl, uint8_t status0);
1.1       root      307: 
1.1.1.4 ! root      308: static uint32_t fdctrl_read_statusA (fdctrl_t *fdctrl);
1.1       root      309: static uint32_t fdctrl_read_statusB (fdctrl_t *fdctrl);
                    310: static uint32_t fdctrl_read_dor (fdctrl_t *fdctrl);
                    311: static void fdctrl_write_dor (fdctrl_t *fdctrl, uint32_t value);
                    312: static uint32_t fdctrl_read_tape (fdctrl_t *fdctrl);
                    313: static void fdctrl_write_tape (fdctrl_t *fdctrl, uint32_t value);
                    314: static uint32_t fdctrl_read_main_status (fdctrl_t *fdctrl);
                    315: static void fdctrl_write_rate (fdctrl_t *fdctrl, uint32_t value);
                    316: static uint32_t fdctrl_read_data (fdctrl_t *fdctrl);
                    317: static void fdctrl_write_data (fdctrl_t *fdctrl, uint32_t value);
                    318: static uint32_t fdctrl_read_dir (fdctrl_t *fdctrl);
                    319: 
                    320: enum {
                    321:     FD_DIR_WRITE   = 0,
                    322:     FD_DIR_READ    = 1,
                    323:     FD_DIR_SCANE   = 2,
                    324:     FD_DIR_SCANL   = 3,
                    325:     FD_DIR_SCANH   = 4,
                    326: };
                    327: 
                    328: enum {
1.1.1.4 ! root      329:     FD_STATE_MULTI  = 0x01,    /* multi track flag */
        !           330:     FD_STATE_FORMAT = 0x02,    /* format flag */
        !           331:     FD_STATE_SEEK   = 0x04,    /* seek flag */
        !           332: };
        !           333: 
        !           334: enum {
        !           335:     FD_REG_SRA = 0x00,
        !           336:     FD_REG_SRB = 0x01,
        !           337:     FD_REG_DOR = 0x02,
        !           338:     FD_REG_TDR = 0x03,
        !           339:     FD_REG_MSR = 0x04,
        !           340:     FD_REG_DSR = 0x04,
        !           341:     FD_REG_FIFO = 0x05,
        !           342:     FD_REG_DIR = 0x07,
        !           343: };
        !           344: 
        !           345: enum {
        !           346:     FD_CMD_READ_TRACK = 0x02,
        !           347:     FD_CMD_SPECIFY = 0x03,
        !           348:     FD_CMD_SENSE_DRIVE_STATUS = 0x04,
        !           349:     FD_CMD_WRITE = 0x05,
        !           350:     FD_CMD_READ = 0x06,
        !           351:     FD_CMD_RECALIBRATE = 0x07,
        !           352:     FD_CMD_SENSE_INTERRUPT_STATUS = 0x08,
        !           353:     FD_CMD_WRITE_DELETED = 0x09,
        !           354:     FD_CMD_READ_ID = 0x0a,
        !           355:     FD_CMD_READ_DELETED = 0x0c,
        !           356:     FD_CMD_FORMAT_TRACK = 0x0d,
        !           357:     FD_CMD_DUMPREG = 0x0e,
        !           358:     FD_CMD_SEEK = 0x0f,
        !           359:     FD_CMD_VERSION = 0x10,
        !           360:     FD_CMD_SCAN_EQUAL = 0x11,
        !           361:     FD_CMD_PERPENDICULAR_MODE = 0x12,
        !           362:     FD_CMD_CONFIGURE = 0x13,
        !           363:     FD_CMD_LOCK = 0x14,
        !           364:     FD_CMD_VERIFY = 0x16,
        !           365:     FD_CMD_POWERDOWN_MODE = 0x17,
        !           366:     FD_CMD_PART_ID = 0x18,
        !           367:     FD_CMD_SCAN_LOW_OR_EQUAL = 0x19,
        !           368:     FD_CMD_SCAN_HIGH_OR_EQUAL = 0x1d,
        !           369:     FD_CMD_SAVE = 0x2c,
        !           370:     FD_CMD_OPTION = 0x33,
        !           371:     FD_CMD_RESTORE = 0x4c,
        !           372:     FD_CMD_DRIVE_SPECIFICATION_COMMAND = 0x8e,
        !           373:     FD_CMD_RELATIVE_SEEK_OUT = 0x8f,
        !           374:     FD_CMD_FORMAT_AND_WRITE = 0xcd,
        !           375:     FD_CMD_RELATIVE_SEEK_IN = 0xcf,
        !           376: };
        !           377: 
        !           378: enum {
        !           379:     FD_CONFIG_PRETRK = 0xff, /* Pre-compensation set to track 0 */
        !           380:     FD_CONFIG_FIFOTHR = 0x0f, /* FIFO threshold set to 1 byte */
        !           381:     FD_CONFIG_POLL  = 0x10, /* Poll enabled */
        !           382:     FD_CONFIG_EFIFO = 0x20, /* FIFO disabled */
        !           383:     FD_CONFIG_EIS   = 0x40, /* No implied seeks */
        !           384: };
        !           385: 
        !           386: enum {
        !           387:     FD_SR0_EQPMT    = 0x10,
        !           388:     FD_SR0_SEEK     = 0x20,
        !           389:     FD_SR0_ABNTERM  = 0x40,
        !           390:     FD_SR0_INVCMD   = 0x80,
        !           391:     FD_SR0_RDYCHG   = 0xc0,
        !           392: };
        !           393: 
        !           394: enum {
        !           395:     FD_SR1_EC       = 0x80, /* End of cylinder */
        !           396: };
        !           397: 
        !           398: enum {
        !           399:     FD_SR2_SNS      = 0x04, /* Scan not satisfied */
        !           400:     FD_SR2_SEH      = 0x08, /* Scan equal hit */
        !           401: };
        !           402: 
        !           403: enum {
        !           404:     FD_SRA_DIR      = 0x01,
        !           405:     FD_SRA_nWP      = 0x02,
        !           406:     FD_SRA_nINDX    = 0x04,
        !           407:     FD_SRA_HDSEL    = 0x08,
        !           408:     FD_SRA_nTRK0    = 0x10,
        !           409:     FD_SRA_STEP     = 0x20,
        !           410:     FD_SRA_nDRV2    = 0x40,
        !           411:     FD_SRA_INTPEND  = 0x80,
        !           412: };
        !           413: 
        !           414: enum {
        !           415:     FD_SRB_MTR0     = 0x01,
        !           416:     FD_SRB_MTR1     = 0x02,
        !           417:     FD_SRB_WGATE    = 0x04,
        !           418:     FD_SRB_RDATA    = 0x08,
        !           419:     FD_SRB_WDATA    = 0x10,
        !           420:     FD_SRB_DR0      = 0x20,
        !           421: };
        !           422: 
        !           423: enum {
        !           424: #if MAX_FD == 4
        !           425:     FD_DOR_SELMASK  = 0x03,
        !           426: #else
        !           427:     FD_DOR_SELMASK  = 0x01,
        !           428: #endif
        !           429:     FD_DOR_nRESET   = 0x04,
        !           430:     FD_DOR_DMAEN    = 0x08,
        !           431:     FD_DOR_MOTEN0   = 0x10,
        !           432:     FD_DOR_MOTEN1   = 0x20,
        !           433:     FD_DOR_MOTEN2   = 0x40,
        !           434:     FD_DOR_MOTEN3   = 0x80,
        !           435: };
        !           436: 
        !           437: enum {
        !           438: #if MAX_FD == 4
        !           439:     FD_TDR_BOOTSEL  = 0x0c,
        !           440: #else
        !           441:     FD_TDR_BOOTSEL  = 0x04,
        !           442: #endif
        !           443: };
        !           444: 
        !           445: enum {
        !           446:     FD_DSR_DRATEMASK= 0x03,
        !           447:     FD_DSR_PWRDOWN  = 0x40,
        !           448:     FD_DSR_SWRESET  = 0x80,
        !           449: };
        !           450: 
        !           451: enum {
        !           452:     FD_MSR_DRV0BUSY = 0x01,
        !           453:     FD_MSR_DRV1BUSY = 0x02,
        !           454:     FD_MSR_DRV2BUSY = 0x04,
        !           455:     FD_MSR_DRV3BUSY = 0x08,
        !           456:     FD_MSR_CMDBUSY  = 0x10,
        !           457:     FD_MSR_NONDMA   = 0x20,
        !           458:     FD_MSR_DIO      = 0x40,
        !           459:     FD_MSR_RQM      = 0x80,
        !           460: };
        !           461: 
        !           462: enum {
        !           463:     FD_DIR_DSKCHG   = 0x80,
1.1       root      464: };
                    465: 
                    466: #define FD_MULTI_TRACK(state) ((state) & FD_STATE_MULTI)
                    467: #define FD_DID_SEEK(state) ((state) & FD_STATE_SEEK)
                    468: #define FD_FORMAT_CMD(state) ((state) & FD_STATE_FORMAT)
                    469: 
                    470: struct fdctrl_t {
                    471:     /* Controller's identification */
                    472:     uint8_t version;
                    473:     /* HW */
1.1.1.3   root      474:     qemu_irq irq;
1.1       root      475:     int dma_chann;
1.1.1.3   root      476:     target_phys_addr_t io_base;
1.1       root      477:     /* Controller state */
                    478:     QEMUTimer *result_timer;
1.1.1.4 ! root      479:     uint8_t sra;
        !           480:     uint8_t srb;
        !           481:     uint8_t dor;
        !           482:     uint8_t tdr;
        !           483:     uint8_t dsr;
        !           484:     uint8_t msr;
1.1       root      485:     uint8_t cur_drv;
1.1.1.4 ! root      486:     uint8_t status0;
        !           487:     uint8_t status1;
        !           488:     uint8_t status2;
1.1       root      489:     /* Command FIFO */
1.1.1.3   root      490:     uint8_t *fifo;
1.1       root      491:     uint32_t data_pos;
                    492:     uint32_t data_len;
                    493:     uint8_t data_state;
                    494:     uint8_t data_dir;
                    495:     uint8_t eot; /* last wanted sector */
                    496:     /* States kept only to be returned back */
                    497:     /* Timers state */
                    498:     uint8_t timer0;
                    499:     uint8_t timer1;
                    500:     /* precompensation */
                    501:     uint8_t precomp_trk;
                    502:     uint8_t config;
                    503:     uint8_t lock;
                    504:     /* Power down config (also with status regB access mode */
                    505:     uint8_t pwrd;
1.1.1.3   root      506:     /* Sun4m quirks? */
                    507:     int sun4m;
1.1       root      508:     /* Floppy drives */
1.1.1.4 ! root      509:     fdrive_t drives[MAX_FD];
        !           510:     int reset_sensei;
1.1       root      511: };
                    512: 
                    513: static uint32_t fdctrl_read (void *opaque, uint32_t reg)
                    514: {
                    515:     fdctrl_t *fdctrl = opaque;
                    516:     uint32_t retval;
                    517: 
1.1.1.4 ! root      518:     switch (reg) {
        !           519:     case FD_REG_SRA:
        !           520:         retval = fdctrl_read_statusA(fdctrl);
1.1.1.3   root      521:         break;
1.1.1.4 ! root      522:     case FD_REG_SRB:
1.1.1.3   root      523:         retval = fdctrl_read_statusB(fdctrl);
                    524:         break;
1.1.1.4 ! root      525:     case FD_REG_DOR:
1.1.1.3   root      526:         retval = fdctrl_read_dor(fdctrl);
                    527:         break;
1.1.1.4 ! root      528:     case FD_REG_TDR:
1.1       root      529:         retval = fdctrl_read_tape(fdctrl);
1.1.1.3   root      530:         break;
1.1.1.4 ! root      531:     case FD_REG_MSR:
1.1       root      532:         retval = fdctrl_read_main_status(fdctrl);
1.1.1.3   root      533:         break;
1.1.1.4 ! root      534:     case FD_REG_FIFO:
1.1       root      535:         retval = fdctrl_read_data(fdctrl);
1.1.1.3   root      536:         break;
1.1.1.4 ! root      537:     case FD_REG_DIR:
1.1       root      538:         retval = fdctrl_read_dir(fdctrl);
1.1.1.3   root      539:         break;
1.1       root      540:     default:
1.1.1.3   root      541:         retval = (uint32_t)(-1);
                    542:         break;
1.1       root      543:     }
                    544:     FLOPPY_DPRINTF("read reg%d: 0x%02x\n", reg & 7, retval);
                    545: 
                    546:     return retval;
                    547: }
                    548: 
                    549: static void fdctrl_write (void *opaque, uint32_t reg, uint32_t value)
                    550: {
                    551:     fdctrl_t *fdctrl = opaque;
                    552: 
                    553:     FLOPPY_DPRINTF("write reg%d: 0x%02x\n", reg & 7, value);
                    554: 
1.1.1.4 ! root      555:     switch (reg) {
        !           556:     case FD_REG_DOR:
1.1.1.3   root      557:         fdctrl_write_dor(fdctrl, value);
                    558:         break;
1.1.1.4 ! root      559:     case FD_REG_TDR:
1.1       root      560:         fdctrl_write_tape(fdctrl, value);
1.1.1.3   root      561:         break;
1.1.1.4 ! root      562:     case FD_REG_DSR:
1.1       root      563:         fdctrl_write_rate(fdctrl, value);
1.1.1.3   root      564:         break;
1.1.1.4 ! root      565:     case FD_REG_FIFO:
1.1       root      566:         fdctrl_write_data(fdctrl, value);
1.1.1.3   root      567:         break;
1.1       root      568:     default:
1.1.1.3   root      569:         break;
1.1       root      570:     }
                    571: }
                    572: 
1.1.1.4 ! root      573: static uint32_t fdctrl_read_port (void *opaque, uint32_t reg)
        !           574: {
        !           575:     return fdctrl_read(opaque, reg & 7);
        !           576: }
        !           577: 
        !           578: static void fdctrl_write_port (void *opaque, uint32_t reg, uint32_t value)
        !           579: {
        !           580:     fdctrl_write(opaque, reg & 7, value);
        !           581: }
        !           582: 
1.1       root      583: static uint32_t fdctrl_read_mem (void *opaque, target_phys_addr_t reg)
                    584: {
1.1.1.3   root      585:     return fdctrl_read(opaque, (uint32_t)reg);
1.1       root      586: }
                    587: 
1.1.1.3   root      588: static void fdctrl_write_mem (void *opaque,
1.1       root      589:                               target_phys_addr_t reg, uint32_t value)
                    590: {
1.1.1.3   root      591:     fdctrl_write(opaque, (uint32_t)reg, value);
1.1       root      592: }
                    593: 
                    594: static CPUReadMemoryFunc *fdctrl_mem_read[3] = {
                    595:     fdctrl_read_mem,
                    596:     fdctrl_read_mem,
                    597:     fdctrl_read_mem,
                    598: };
                    599: 
                    600: static CPUWriteMemoryFunc *fdctrl_mem_write[3] = {
                    601:     fdctrl_write_mem,
                    602:     fdctrl_write_mem,
                    603:     fdctrl_write_mem,
                    604: };
                    605: 
1.1.1.3   root      606: static CPUReadMemoryFunc *fdctrl_mem_read_strict[3] = {
                    607:     fdctrl_read_mem,
                    608:     NULL,
                    609:     NULL,
                    610: };
                    611: 
                    612: static CPUWriteMemoryFunc *fdctrl_mem_write_strict[3] = {
                    613:     fdctrl_write_mem,
                    614:     NULL,
                    615:     NULL,
                    616: };
                    617: 
                    618: static void fd_save (QEMUFile *f, fdrive_t *fd)
                    619: {
                    620:     qemu_put_8s(f, &fd->head);
                    621:     qemu_put_8s(f, &fd->track);
                    622:     qemu_put_8s(f, &fd->sect);
                    623: }
                    624: 
                    625: static void fdc_save (QEMUFile *f, void *opaque)
                    626: {
                    627:     fdctrl_t *s = opaque;
1.1.1.4 ! root      628:     uint8_t tmp;
        !           629:     int i;
        !           630:     uint8_t dor = s->dor | GET_CUR_DRV(s);
1.1.1.3   root      631: 
1.1.1.4 ! root      632:     /* Controller state */
        !           633:     qemu_put_8s(f, &s->sra);
        !           634:     qemu_put_8s(f, &s->srb);
        !           635:     qemu_put_8s(f, &dor);
        !           636:     qemu_put_8s(f, &s->tdr);
        !           637:     qemu_put_8s(f, &s->dsr);
        !           638:     qemu_put_8s(f, &s->msr);
        !           639:     qemu_put_8s(f, &s->status0);
        !           640:     qemu_put_8s(f, &s->status1);
        !           641:     qemu_put_8s(f, &s->status2);
        !           642:     /* Command FIFO */
1.1.1.3   root      643:     qemu_put_buffer(f, s->fifo, FD_SECTOR_LEN);
                    644:     qemu_put_be32s(f, &s->data_pos);
                    645:     qemu_put_be32s(f, &s->data_len);
                    646:     qemu_put_8s(f, &s->data_state);
                    647:     qemu_put_8s(f, &s->data_dir);
                    648:     qemu_put_8s(f, &s->eot);
1.1.1.4 ! root      649:     /* States kept only to be returned back */
1.1.1.3   root      650:     qemu_put_8s(f, &s->timer0);
                    651:     qemu_put_8s(f, &s->timer1);
                    652:     qemu_put_8s(f, &s->precomp_trk);
                    653:     qemu_put_8s(f, &s->config);
                    654:     qemu_put_8s(f, &s->lock);
                    655:     qemu_put_8s(f, &s->pwrd);
1.1.1.4 ! root      656: 
        !           657:     tmp = MAX_FD;
        !           658:     qemu_put_8s(f, &tmp);
        !           659:     for (i = 0; i < MAX_FD; i++)
        !           660:         fd_save(f, &s->drives[i]);
1.1.1.3   root      661: }
                    662: 
                    663: static int fd_load (QEMUFile *f, fdrive_t *fd)
                    664: {
                    665:     qemu_get_8s(f, &fd->head);
                    666:     qemu_get_8s(f, &fd->track);
                    667:     qemu_get_8s(f, &fd->sect);
                    668: 
                    669:     return 0;
                    670: }
                    671: 
                    672: static int fdc_load (QEMUFile *f, void *opaque, int version_id)
                    673: {
                    674:     fdctrl_t *s = opaque;
1.1.1.4 ! root      675:     int i, ret = 0;
        !           676:     uint8_t n;
1.1.1.3   root      677: 
1.1.1.4 ! root      678:     if (version_id != 2)
1.1.1.3   root      679:         return -EINVAL;
                    680: 
1.1.1.4 ! root      681:     /* Controller state */
        !           682:     qemu_get_8s(f, &s->sra);
        !           683:     qemu_get_8s(f, &s->srb);
        !           684:     qemu_get_8s(f, &s->dor);
        !           685:     SET_CUR_DRV(s, s->dor & FD_DOR_SELMASK);
        !           686:     s->dor &= ~FD_DOR_SELMASK;
        !           687:     qemu_get_8s(f, &s->tdr);
        !           688:     qemu_get_8s(f, &s->dsr);
        !           689:     qemu_get_8s(f, &s->msr);
        !           690:     qemu_get_8s(f, &s->status0);
        !           691:     qemu_get_8s(f, &s->status1);
        !           692:     qemu_get_8s(f, &s->status2);
        !           693:     /* Command FIFO */
1.1.1.3   root      694:     qemu_get_buffer(f, s->fifo, FD_SECTOR_LEN);
                    695:     qemu_get_be32s(f, &s->data_pos);
                    696:     qemu_get_be32s(f, &s->data_len);
                    697:     qemu_get_8s(f, &s->data_state);
                    698:     qemu_get_8s(f, &s->data_dir);
                    699:     qemu_get_8s(f, &s->eot);
1.1.1.4 ! root      700:     /* States kept only to be returned back */
1.1.1.3   root      701:     qemu_get_8s(f, &s->timer0);
                    702:     qemu_get_8s(f, &s->timer1);
                    703:     qemu_get_8s(f, &s->precomp_trk);
                    704:     qemu_get_8s(f, &s->config);
                    705:     qemu_get_8s(f, &s->lock);
                    706:     qemu_get_8s(f, &s->pwrd);
1.1.1.4 ! root      707:     qemu_get_8s(f, &n);
1.1.1.3   root      708: 
1.1.1.4 ! root      709:     if (n > MAX_FD)
        !           710:         return -EINVAL;
        !           711: 
        !           712:     for (i = 0; i < n; i++) {
        !           713:         ret = fd_load(f, &s->drives[i]);
        !           714:         if (ret != 0)
        !           715:             break;
        !           716:     }
1.1.1.3   root      717: 
                    718:     return ret;
                    719: }
                    720: 
                    721: static void fdctrl_external_reset(void *opaque)
                    722: {
                    723:     fdctrl_t *s = opaque;
                    724: 
                    725:     fdctrl_reset(s, 0);
                    726: }
                    727: 
1.1.1.4 ! root      728: static void fdctrl_handle_tc(void *opaque, int irq, int level)
1.1.1.3   root      729: {
1.1.1.4 ! root      730:     //fdctrl_t *s = opaque;
1.1.1.3   root      731: 
1.1.1.4 ! root      732:     if (level) {
        !           733:         // XXX
        !           734:         FLOPPY_DPRINTF("TC pulsed\n");
1.1       root      735:     }
1.1.1.3   root      736: }
                    737: 
1.1       root      738: /* XXX: may change if moved to bdrv */
                    739: int fdctrl_get_drive_type(fdctrl_t *fdctrl, int drive_num)
                    740: {
                    741:     return fdctrl->drives[drive_num].drive;
                    742: }
                    743: 
                    744: /* Change IRQ state */
                    745: static void fdctrl_reset_irq (fdctrl_t *fdctrl)
                    746: {
1.1.1.4 ! root      747:     if (!(fdctrl->sra & FD_SRA_INTPEND))
        !           748:         return;
1.1       root      749:     FLOPPY_DPRINTF("Reset interrupt\n");
1.1.1.3   root      750:     qemu_set_irq(fdctrl->irq, 0);
1.1.1.4 ! root      751:     fdctrl->sra &= ~FD_SRA_INTPEND;
1.1       root      752: }
                    753: 
1.1.1.4 ! root      754: static void fdctrl_raise_irq (fdctrl_t *fdctrl, uint8_t status0)
1.1       root      755: {
1.1.1.4 ! root      756:     /* Sparc mutation */
        !           757:     if (fdctrl->sun4m && (fdctrl->msr & FD_MSR_CMDBUSY)) {
        !           758:         /* XXX: not sure */
        !           759:         fdctrl->msr &= ~FD_MSR_CMDBUSY;
        !           760:         fdctrl->msr |= FD_MSR_RQM | FD_MSR_DIO;
        !           761:         fdctrl->status0 = status0;
1.1.1.3   root      762:         return;
1.1       root      763:     }
1.1.1.4 ! root      764:     if (!(fdctrl->sra & FD_SRA_INTPEND)) {
1.1.1.3   root      765:         qemu_set_irq(fdctrl->irq, 1);
1.1.1.4 ! root      766:         fdctrl->sra |= FD_SRA_INTPEND;
1.1       root      767:     }
1.1.1.4 ! root      768:     fdctrl->reset_sensei = 0;
        !           769:     fdctrl->status0 = status0;
        !           770:     FLOPPY_DPRINTF("Set interrupt status to 0x%02x\n", fdctrl->status0);
1.1       root      771: }
                    772: 
                    773: /* Reset controller */
                    774: static void fdctrl_reset (fdctrl_t *fdctrl, int do_irq)
                    775: {
                    776:     int i;
                    777: 
                    778:     FLOPPY_DPRINTF("reset controller\n");
                    779:     fdctrl_reset_irq(fdctrl);
                    780:     /* Initialise controller */
1.1.1.4 ! root      781:     fdctrl->sra = 0;
        !           782:     fdctrl->srb = 0xc0;
        !           783:     if (!fdctrl->drives[1].bs)
        !           784:         fdctrl->sra |= FD_SRA_nDRV2;
1.1       root      785:     fdctrl->cur_drv = 0;
1.1.1.4 ! root      786:     fdctrl->dor = FD_DOR_nRESET;
        !           787:     fdctrl->dor |= (fdctrl->dma_chann != -1) ? FD_DOR_DMAEN : 0;
        !           788:     fdctrl->msr = FD_MSR_RQM;
1.1       root      789:     /* FIFO state */
                    790:     fdctrl->data_pos = 0;
                    791:     fdctrl->data_len = 0;
1.1.1.4 ! root      792:     fdctrl->data_state = 0;
1.1       root      793:     fdctrl->data_dir = FD_DIR_WRITE;
                    794:     for (i = 0; i < MAX_FD; i++)
1.1.1.4 ! root      795:         fd_recalibrate(&fdctrl->drives[i]);
1.1       root      796:     fdctrl_reset_fifo(fdctrl);
1.1.1.4 ! root      797:     if (do_irq) {
        !           798:         fdctrl_raise_irq(fdctrl, FD_SR0_RDYCHG);
        !           799:         fdctrl->reset_sensei = FD_RESET_SENSEI_COUNT;
        !           800:     }
1.1       root      801: }
                    802: 
                    803: static inline fdrive_t *drv0 (fdctrl_t *fdctrl)
                    804: {
1.1.1.4 ! root      805:     return &fdctrl->drives[(fdctrl->tdr & FD_TDR_BOOTSEL) >> 2];
1.1       root      806: }
                    807: 
                    808: static inline fdrive_t *drv1 (fdctrl_t *fdctrl)
                    809: {
1.1.1.4 ! root      810:     if ((fdctrl->tdr & FD_TDR_BOOTSEL) < (1 << 2))
        !           811:         return &fdctrl->drives[1];
        !           812:     else
        !           813:         return &fdctrl->drives[0];
        !           814: }
        !           815: 
        !           816: #if MAX_FD == 4
        !           817: static inline fdrive_t *drv2 (fdctrl_t *fdctrl)
        !           818: {
        !           819:     if ((fdctrl->tdr & FD_TDR_BOOTSEL) < (2 << 2))
        !           820:         return &fdctrl->drives[2];
        !           821:     else
        !           822:         return &fdctrl->drives[1];
        !           823: }
        !           824: 
        !           825: static inline fdrive_t *drv3 (fdctrl_t *fdctrl)
        !           826: {
        !           827:     if ((fdctrl->tdr & FD_TDR_BOOTSEL) < (3 << 2))
        !           828:         return &fdctrl->drives[3];
        !           829:     else
        !           830:         return &fdctrl->drives[2];
1.1       root      831: }
1.1.1.4 ! root      832: #endif
1.1       root      833: 
                    834: static fdrive_t *get_cur_drv (fdctrl_t *fdctrl)
                    835: {
1.1.1.4 ! root      836:     switch (fdctrl->cur_drv) {
        !           837:         case 0: return drv0(fdctrl);
        !           838:         case 1: return drv1(fdctrl);
        !           839: #if MAX_FD == 4
        !           840:         case 2: return drv2(fdctrl);
        !           841:         case 3: return drv3(fdctrl);
        !           842: #endif
        !           843:         default: return NULL;
        !           844:     }
        !           845: }
        !           846: 
        !           847: /* Status A register : 0x00 (read-only) */
        !           848: static uint32_t fdctrl_read_statusA (fdctrl_t *fdctrl)
        !           849: {
        !           850:     uint32_t retval = fdctrl->sra;
        !           851: 
        !           852:     FLOPPY_DPRINTF("status register A: 0x%02x\n", retval);
        !           853: 
        !           854:     return retval;
1.1       root      855: }
                    856: 
                    857: /* Status B register : 0x01 (read-only) */
                    858: static uint32_t fdctrl_read_statusB (fdctrl_t *fdctrl)
                    859: {
1.1.1.4 ! root      860:     uint32_t retval = fdctrl->srb;
        !           861: 
        !           862:     FLOPPY_DPRINTF("status register B: 0x%02x\n", retval);
        !           863: 
        !           864:     return retval;
1.1       root      865: }
                    866: 
                    867: /* Digital output register : 0x02 */
                    868: static uint32_t fdctrl_read_dor (fdctrl_t *fdctrl)
                    869: {
1.1.1.4 ! root      870:     uint32_t retval = fdctrl->dor;
1.1       root      871: 
                    872:     /* Selected drive */
                    873:     retval |= fdctrl->cur_drv;
                    874:     FLOPPY_DPRINTF("digital output register: 0x%02x\n", retval);
                    875: 
                    876:     return retval;
                    877: }
                    878: 
                    879: static void fdctrl_write_dor (fdctrl_t *fdctrl, uint32_t value)
                    880: {
                    881:     FLOPPY_DPRINTF("digital output register set to 0x%02x\n", value);
1.1.1.4 ! root      882: 
        !           883:     /* Motors */
        !           884:     if (value & FD_DOR_MOTEN0)
        !           885:         fdctrl->srb |= FD_SRB_MTR0;
1.1       root      886:     else
1.1.1.4 ! root      887:         fdctrl->srb &= ~FD_SRB_MTR0;
        !           888:     if (value & FD_DOR_MOTEN1)
        !           889:         fdctrl->srb |= FD_SRB_MTR1;
1.1       root      890:     else
1.1.1.4 ! root      891:         fdctrl->srb &= ~FD_SRB_MTR1;
        !           892: 
        !           893:     /* Drive */
        !           894:     if (value & 1)
        !           895:         fdctrl->srb |= FD_SRB_DR0;
        !           896:     else
        !           897:         fdctrl->srb &= ~FD_SRB_DR0;
        !           898: 
1.1       root      899:     /* Reset */
1.1.1.4 ! root      900:     if (!(value & FD_DOR_nRESET)) {
        !           901:         if (fdctrl->dor & FD_DOR_nRESET) {
1.1       root      902:             FLOPPY_DPRINTF("controller enter RESET state\n");
                    903:         }
                    904:     } else {
1.1.1.4 ! root      905:         if (!(fdctrl->dor & FD_DOR_nRESET)) {
1.1       root      906:             FLOPPY_DPRINTF("controller out of RESET state\n");
                    907:             fdctrl_reset(fdctrl, 1);
1.1.1.4 ! root      908:             fdctrl->dsr &= ~FD_DSR_PWRDOWN;
1.1       root      909:         }
                    910:     }
                    911:     /* Selected drive */
1.1.1.4 ! root      912:     fdctrl->cur_drv = value & FD_DOR_SELMASK;
        !           913: 
        !           914:     fdctrl->dor = value;
1.1       root      915: }
                    916: 
                    917: /* Tape drive register : 0x03 */
                    918: static uint32_t fdctrl_read_tape (fdctrl_t *fdctrl)
                    919: {
1.1.1.4 ! root      920:     uint32_t retval = fdctrl->tdr;
1.1       root      921: 
                    922:     FLOPPY_DPRINTF("tape drive register: 0x%02x\n", retval);
                    923: 
                    924:     return retval;
                    925: }
                    926: 
                    927: static void fdctrl_write_tape (fdctrl_t *fdctrl, uint32_t value)
                    928: {
                    929:     /* Reset mode */
1.1.1.4 ! root      930:     if (!(fdctrl->dor & FD_DOR_nRESET)) {
1.1       root      931:         FLOPPY_DPRINTF("Floppy controller in RESET state !\n");
                    932:         return;
                    933:     }
                    934:     FLOPPY_DPRINTF("tape drive register set to 0x%02x\n", value);
                    935:     /* Disk boot selection indicator */
1.1.1.4 ! root      936:     fdctrl->tdr = value & FD_TDR_BOOTSEL;
1.1       root      937:     /* Tape indicators: never allow */
                    938: }
                    939: 
                    940: /* Main status register : 0x04 (read) */
                    941: static uint32_t fdctrl_read_main_status (fdctrl_t *fdctrl)
                    942: {
1.1.1.4 ! root      943:     uint32_t retval = fdctrl->msr;
        !           944: 
        !           945:     fdctrl->dsr &= ~FD_DSR_PWRDOWN;
        !           946:     fdctrl->dor |= FD_DOR_nRESET;
1.1       root      947: 
                    948:     FLOPPY_DPRINTF("main status register: 0x%02x\n", retval);
                    949: 
                    950:     return retval;
                    951: }
                    952: 
                    953: /* Data select rate register : 0x04 (write) */
                    954: static void fdctrl_write_rate (fdctrl_t *fdctrl, uint32_t value)
                    955: {
                    956:     /* Reset mode */
1.1.1.4 ! root      957:     if (!(fdctrl->dor & FD_DOR_nRESET)) {
1.1.1.3   root      958:         FLOPPY_DPRINTF("Floppy controller in RESET state !\n");
                    959:         return;
                    960:     }
1.1       root      961:     FLOPPY_DPRINTF("select rate register set to 0x%02x\n", value);
                    962:     /* Reset: autoclear */
1.1.1.4 ! root      963:     if (value & FD_DSR_SWRESET) {
        !           964:         fdctrl->dor &= ~FD_DOR_nRESET;
1.1       root      965:         fdctrl_reset(fdctrl, 1);
1.1.1.4 ! root      966:         fdctrl->dor |= FD_DOR_nRESET;
1.1       root      967:     }
1.1.1.4 ! root      968:     if (value & FD_DSR_PWRDOWN) {
1.1       root      969:         fdctrl_reset(fdctrl, 1);
                    970:     }
1.1.1.4 ! root      971:     fdctrl->dsr = value;
1.1       root      972: }
                    973: 
1.1.1.2   root      974: static int fdctrl_media_changed(fdrive_t *drv)
                    975: {
                    976:     int ret;
1.1.1.3   root      977: 
                    978:     if (!drv->bs)
1.1.1.2   root      979:         return 0;
                    980:     ret = bdrv_media_changed(drv->bs);
                    981:     if (ret) {
                    982:         fd_revalidate(drv);
                    983:     }
                    984:     return ret;
                    985: }
                    986: 
1.1       root      987: /* Digital input register : 0x07 (read-only) */
                    988: static uint32_t fdctrl_read_dir (fdctrl_t *fdctrl)
                    989: {
                    990:     uint32_t retval = 0;
                    991: 
1.1.1.4 ! root      992:     if (fdctrl_media_changed(drv0(fdctrl))
        !           993:      || fdctrl_media_changed(drv1(fdctrl))
        !           994: #if MAX_FD == 4
        !           995:      || fdctrl_media_changed(drv2(fdctrl))
        !           996:      || fdctrl_media_changed(drv3(fdctrl))
        !           997: #endif
        !           998:         )
        !           999:         retval |= FD_DIR_DSKCHG;
1.1       root     1000:     if (retval != 0)
                   1001:         FLOPPY_DPRINTF("Floppy digital input register: 0x%02x\n", retval);
                   1002: 
                   1003:     return retval;
                   1004: }
                   1005: 
                   1006: /* FIFO state control */
                   1007: static void fdctrl_reset_fifo (fdctrl_t *fdctrl)
                   1008: {
                   1009:     fdctrl->data_dir = FD_DIR_WRITE;
                   1010:     fdctrl->data_pos = 0;
1.1.1.4 ! root     1011:     fdctrl->msr &= ~(FD_MSR_CMDBUSY | FD_MSR_DIO);
1.1       root     1012: }
                   1013: 
                   1014: /* Set FIFO status for the host to read */
                   1015: static void fdctrl_set_fifo (fdctrl_t *fdctrl, int fifo_len, int do_irq)
                   1016: {
                   1017:     fdctrl->data_dir = FD_DIR_READ;
                   1018:     fdctrl->data_len = fifo_len;
                   1019:     fdctrl->data_pos = 0;
1.1.1.4 ! root     1020:     fdctrl->msr |= FD_MSR_CMDBUSY | FD_MSR_RQM | FD_MSR_DIO;
1.1       root     1021:     if (do_irq)
                   1022:         fdctrl_raise_irq(fdctrl, 0x00);
                   1023: }
                   1024: 
                   1025: /* Set an error: unimplemented/unknown command */
1.1.1.4 ! root     1026: static void fdctrl_unimplemented (fdctrl_t *fdctrl, int direction)
1.1       root     1027: {
1.1.1.4 ! root     1028:     FLOPPY_ERROR("unimplemented command 0x%02x\n", fdctrl->fifo[0]);
        !          1029:     fdctrl->fifo[0] = FD_SR0_INVCMD;
1.1       root     1030:     fdctrl_set_fifo(fdctrl, 1, 0);
1.1.1.4 ! root     1031: }
        !          1032: 
        !          1033: /* Seek to next sector */
        !          1034: static int fdctrl_seek_to_next_sect (fdctrl_t *fdctrl, fdrive_t *cur_drv)
        !          1035: {
        !          1036:     FLOPPY_DPRINTF("seek to next sector (%d %02x %02x => %d)\n",
        !          1037:                    cur_drv->head, cur_drv->track, cur_drv->sect,
        !          1038:                    fd_sector(cur_drv));
        !          1039:     /* XXX: cur_drv->sect >= cur_drv->last_sect should be an
        !          1040:        error in fact */
        !          1041:     if (cur_drv->sect >= cur_drv->last_sect ||
        !          1042:         cur_drv->sect == fdctrl->eot) {
        !          1043:         cur_drv->sect = 1;
        !          1044:         if (FD_MULTI_TRACK(fdctrl->data_state)) {
        !          1045:             if (cur_drv->head == 0 &&
        !          1046:                 (cur_drv->flags & FDISK_DBL_SIDES) != 0) {
        !          1047:                 cur_drv->head = 1;
        !          1048:             } else {
        !          1049:                 cur_drv->head = 0;
        !          1050:                 cur_drv->track++;
        !          1051:                 if ((cur_drv->flags & FDISK_DBL_SIDES) == 0)
        !          1052:                     return 0;
        !          1053:             }
        !          1054:         } else {
        !          1055:             cur_drv->track++;
        !          1056:             return 0;
        !          1057:         }
        !          1058:         FLOPPY_DPRINTF("seek to next track (%d %02x %02x => %d)\n",
        !          1059:                        cur_drv->head, cur_drv->track,
        !          1060:                        cur_drv->sect, fd_sector(cur_drv));
        !          1061:     } else {
        !          1062:         cur_drv->sect++;
        !          1063:     }
        !          1064:     return 1;
1.1       root     1065: }
                   1066: 
                   1067: /* Callback for transfer end (stop or abort) */
                   1068: static void fdctrl_stop_transfer (fdctrl_t *fdctrl, uint8_t status0,
1.1.1.3   root     1069:                                   uint8_t status1, uint8_t status2)
1.1       root     1070: {
                   1071:     fdrive_t *cur_drv;
                   1072: 
                   1073:     cur_drv = get_cur_drv(fdctrl);
                   1074:     FLOPPY_DPRINTF("transfer status: %02x %02x %02x (%02x)\n",
                   1075:                    status0, status1, status2,
1.1.1.4 ! root     1076:                    status0 | (cur_drv->head << 2) | GET_CUR_DRV(fdctrl));
        !          1077:     fdctrl->fifo[0] = status0 | (cur_drv->head << 2) | GET_CUR_DRV(fdctrl);
1.1       root     1078:     fdctrl->fifo[1] = status1;
                   1079:     fdctrl->fifo[2] = status2;
                   1080:     fdctrl->fifo[3] = cur_drv->track;
                   1081:     fdctrl->fifo[4] = cur_drv->head;
                   1082:     fdctrl->fifo[5] = cur_drv->sect;
                   1083:     fdctrl->fifo[6] = FD_SECTOR_SC;
                   1084:     fdctrl->data_dir = FD_DIR_READ;
1.1.1.4 ! root     1085:     if (!(fdctrl->msr & FD_MSR_NONDMA)) {
1.1       root     1086:         DMA_release_DREQ(fdctrl->dma_chann);
                   1087:     }
1.1.1.4 ! root     1088:     fdctrl->msr |= FD_MSR_RQM | FD_MSR_DIO;
        !          1089:     fdctrl->msr &= ~FD_MSR_NONDMA;
1.1       root     1090:     fdctrl_set_fifo(fdctrl, 7, 1);
                   1091: }
                   1092: 
                   1093: /* Prepare a data transfer (either DMA or FIFO) */
                   1094: static void fdctrl_start_transfer (fdctrl_t *fdctrl, int direction)
                   1095: {
                   1096:     fdrive_t *cur_drv;
                   1097:     uint8_t kh, kt, ks;
1.1.1.4 ! root     1098:     int did_seek = 0;
1.1       root     1099: 
1.1.1.4 ! root     1100:     SET_CUR_DRV(fdctrl, fdctrl->fifo[1] & FD_DOR_SELMASK);
1.1       root     1101:     cur_drv = get_cur_drv(fdctrl);
                   1102:     kt = fdctrl->fifo[2];
                   1103:     kh = fdctrl->fifo[3];
                   1104:     ks = fdctrl->fifo[4];
                   1105:     FLOPPY_DPRINTF("Start transfer at %d %d %02x %02x (%d)\n",
1.1.1.4 ! root     1106:                    GET_CUR_DRV(fdctrl), kh, kt, ks,
1.1       root     1107:                    _fd_sector(kh, kt, ks, cur_drv->last_sect));
1.1.1.4 ! root     1108:     switch (fd_seek(cur_drv, kh, kt, ks, fdctrl->config & FD_CONFIG_EIS)) {
1.1       root     1109:     case 2:
                   1110:         /* sect too big */
1.1.1.4 ! root     1111:         fdctrl_stop_transfer(fdctrl, FD_SR0_ABNTERM, 0x00, 0x00);
1.1       root     1112:         fdctrl->fifo[3] = kt;
                   1113:         fdctrl->fifo[4] = kh;
                   1114:         fdctrl->fifo[5] = ks;
                   1115:         return;
                   1116:     case 3:
                   1117:         /* track too big */
1.1.1.4 ! root     1118:         fdctrl_stop_transfer(fdctrl, FD_SR0_ABNTERM, FD_SR1_EC, 0x00);
1.1       root     1119:         fdctrl->fifo[3] = kt;
                   1120:         fdctrl->fifo[4] = kh;
                   1121:         fdctrl->fifo[5] = ks;
                   1122:         return;
                   1123:     case 4:
                   1124:         /* No seek enabled */
1.1.1.4 ! root     1125:         fdctrl_stop_transfer(fdctrl, FD_SR0_ABNTERM, 0x00, 0x00);
1.1       root     1126:         fdctrl->fifo[3] = kt;
                   1127:         fdctrl->fifo[4] = kh;
                   1128:         fdctrl->fifo[5] = ks;
                   1129:         return;
                   1130:     case 1:
                   1131:         did_seek = 1;
                   1132:         break;
                   1133:     default:
                   1134:         break;
                   1135:     }
1.1.1.4 ! root     1136: 
1.1       root     1137:     /* Set the FIFO state */
                   1138:     fdctrl->data_dir = direction;
                   1139:     fdctrl->data_pos = 0;
1.1.1.4 ! root     1140:     fdctrl->msr |= FD_MSR_CMDBUSY;
1.1       root     1141:     if (fdctrl->fifo[0] & 0x80)
                   1142:         fdctrl->data_state |= FD_STATE_MULTI;
                   1143:     else
                   1144:         fdctrl->data_state &= ~FD_STATE_MULTI;
                   1145:     if (did_seek)
                   1146:         fdctrl->data_state |= FD_STATE_SEEK;
                   1147:     else
                   1148:         fdctrl->data_state &= ~FD_STATE_SEEK;
                   1149:     if (fdctrl->fifo[5] == 00) {
                   1150:         fdctrl->data_len = fdctrl->fifo[8];
                   1151:     } else {
1.1.1.3   root     1152:         int tmp;
1.1.1.2   root     1153:         fdctrl->data_len = 128 << (fdctrl->fifo[5] > 7 ? 7 : fdctrl->fifo[5]);
1.1.1.4 ! root     1154:         tmp = (fdctrl->fifo[6] - ks + 1);
1.1       root     1155:         if (fdctrl->fifo[0] & 0x80)
1.1.1.4 ! root     1156:             tmp += fdctrl->fifo[6];
1.1.1.3   root     1157:         fdctrl->data_len *= tmp;
1.1       root     1158:     }
                   1159:     fdctrl->eot = fdctrl->fifo[6];
1.1.1.4 ! root     1160:     if (fdctrl->dor & FD_DOR_DMAEN) {
1.1       root     1161:         int dma_mode;
                   1162:         /* DMA transfer are enabled. Check if DMA channel is well programmed */
                   1163:         dma_mode = DMA_get_channel_mode(fdctrl->dma_chann);
                   1164:         dma_mode = (dma_mode >> 2) & 3;
                   1165:         FLOPPY_DPRINTF("dma_mode=%d direction=%d (%d - %d)\n",
1.1.1.3   root     1166:                        dma_mode, direction,
1.1       root     1167:                        (128 << fdctrl->fifo[5]) *
1.1.1.3   root     1168:                        (cur_drv->last_sect - ks + 1), fdctrl->data_len);
1.1       root     1169:         if (((direction == FD_DIR_SCANE || direction == FD_DIR_SCANL ||
                   1170:               direction == FD_DIR_SCANH) && dma_mode == 0) ||
                   1171:             (direction == FD_DIR_WRITE && dma_mode == 2) ||
                   1172:             (direction == FD_DIR_READ && dma_mode == 1)) {
                   1173:             /* No access is allowed until DMA transfer has completed */
1.1.1.4 ! root     1174:             fdctrl->msr &= ~FD_MSR_RQM;
1.1       root     1175:             /* Now, we just have to wait for the DMA controller to
                   1176:              * recall us...
                   1177:              */
                   1178:             DMA_hold_DREQ(fdctrl->dma_chann);
                   1179:             DMA_schedule(fdctrl->dma_chann);
                   1180:             return;
                   1181:         } else {
1.1.1.3   root     1182:             FLOPPY_ERROR("dma_mode=%d direction=%d\n", dma_mode, direction);
1.1       root     1183:         }
                   1184:     }
                   1185:     FLOPPY_DPRINTF("start non-DMA transfer\n");
1.1.1.4 ! root     1186:     fdctrl->msr |= FD_MSR_NONDMA;
        !          1187:     if (direction != FD_DIR_WRITE)
        !          1188:         fdctrl->msr |= FD_MSR_DIO;
1.1       root     1189:     /* IO based transfer: calculate len */
                   1190:     fdctrl_raise_irq(fdctrl, 0x00);
                   1191: 
                   1192:     return;
                   1193: }
                   1194: 
                   1195: /* Prepare a transfer of deleted data */
                   1196: static void fdctrl_start_transfer_del (fdctrl_t *fdctrl, int direction)
                   1197: {
1.1.1.4 ! root     1198:     FLOPPY_ERROR("fdctrl_start_transfer_del() unimplemented\n");
        !          1199: 
1.1       root     1200:     /* We don't handle deleted data,
                   1201:      * so we don't return *ANYTHING*
                   1202:      */
1.1.1.4 ! root     1203:     fdctrl_stop_transfer(fdctrl, FD_SR0_ABNTERM | FD_SR0_SEEK, 0x00, 0x00);
1.1       root     1204: }
                   1205: 
                   1206: /* handlers for DMA transfers */
                   1207: static int fdctrl_transfer_handler (void *opaque, int nchan,
                   1208:                                     int dma_pos, int dma_len)
                   1209: {
                   1210:     fdctrl_t *fdctrl;
                   1211:     fdrive_t *cur_drv;
                   1212:     int len, start_pos, rel_pos;
                   1213:     uint8_t status0 = 0x00, status1 = 0x00, status2 = 0x00;
                   1214: 
                   1215:     fdctrl = opaque;
1.1.1.4 ! root     1216:     if (fdctrl->msr & FD_MSR_RQM) {
1.1       root     1217:         FLOPPY_DPRINTF("Not in DMA transfer mode !\n");
                   1218:         return 0;
                   1219:     }
                   1220:     cur_drv = get_cur_drv(fdctrl);
                   1221:     if (fdctrl->data_dir == FD_DIR_SCANE || fdctrl->data_dir == FD_DIR_SCANL ||
                   1222:         fdctrl->data_dir == FD_DIR_SCANH)
1.1.1.4 ! root     1223:         status2 = FD_SR2_SNS;
1.1       root     1224:     if (dma_len > fdctrl->data_len)
                   1225:         dma_len = fdctrl->data_len;
                   1226:     if (cur_drv->bs == NULL) {
1.1.1.3   root     1227:         if (fdctrl->data_dir == FD_DIR_WRITE)
1.1.1.4 ! root     1228:             fdctrl_stop_transfer(fdctrl, FD_SR0_ABNTERM | FD_SR0_SEEK, 0x00, 0x00);
1.1.1.3   root     1229:         else
1.1.1.4 ! root     1230:             fdctrl_stop_transfer(fdctrl, FD_SR0_ABNTERM, 0x00, 0x00);
1.1.1.3   root     1231:         len = 0;
1.1       root     1232:         goto transfer_error;
                   1233:     }
                   1234:     rel_pos = fdctrl->data_pos % FD_SECTOR_LEN;
                   1235:     for (start_pos = fdctrl->data_pos; fdctrl->data_pos < dma_len;) {
                   1236:         len = dma_len - fdctrl->data_pos;
                   1237:         if (len + rel_pos > FD_SECTOR_LEN)
                   1238:             len = FD_SECTOR_LEN - rel_pos;
                   1239:         FLOPPY_DPRINTF("copy %d bytes (%d %d %d) %d pos %d %02x "
                   1240:                        "(%d-0x%08x 0x%08x)\n", len, dma_len, fdctrl->data_pos,
1.1.1.4 ! root     1241:                        fdctrl->data_len, GET_CUR_DRV(fdctrl), cur_drv->head,
1.1       root     1242:                        cur_drv->track, cur_drv->sect, fd_sector(cur_drv),
1.1.1.4 ! root     1243:                        fd_sector(cur_drv) * FD_SECTOR_LEN);
1.1       root     1244:         if (fdctrl->data_dir != FD_DIR_WRITE ||
1.1.1.3   root     1245:             len < FD_SECTOR_LEN || rel_pos != 0) {
1.1       root     1246:             /* READ & SCAN commands and realign to a sector for WRITE */
                   1247:             if (bdrv_read(cur_drv->bs, fd_sector(cur_drv),
1.1.1.3   root     1248:                           fdctrl->fifo, 1) < 0) {
1.1       root     1249:                 FLOPPY_DPRINTF("Floppy: error getting sector %d\n",
                   1250:                                fd_sector(cur_drv));
                   1251:                 /* Sure, image size is too small... */
                   1252:                 memset(fdctrl->fifo, 0, FD_SECTOR_LEN);
                   1253:             }
                   1254:         }
1.1.1.3   root     1255:         switch (fdctrl->data_dir) {
                   1256:         case FD_DIR_READ:
                   1257:             /* READ commands */
1.1       root     1258:             DMA_write_memory (nchan, fdctrl->fifo + rel_pos,
                   1259:                               fdctrl->data_pos, len);
1.1.1.3   root     1260:             break;
                   1261:         case FD_DIR_WRITE:
1.1       root     1262:             /* WRITE commands */
                   1263:             DMA_read_memory (nchan, fdctrl->fifo + rel_pos,
                   1264:                              fdctrl->data_pos, len);
                   1265:             if (bdrv_write(cur_drv->bs, fd_sector(cur_drv),
1.1.1.3   root     1266:                            fdctrl->fifo, 1) < 0) {
1.1.1.4 ! root     1267:                 FLOPPY_ERROR("writing sector %d\n", fd_sector(cur_drv));
        !          1268:                 fdctrl_stop_transfer(fdctrl, FD_SR0_ABNTERM | FD_SR0_SEEK, 0x00, 0x00);
1.1       root     1269:                 goto transfer_error;
                   1270:             }
1.1.1.3   root     1271:             break;
                   1272:         default:
                   1273:             /* SCAN commands */
1.1       root     1274:             {
1.1.1.3   root     1275:                 uint8_t tmpbuf[FD_SECTOR_LEN];
1.1       root     1276:                 int ret;
                   1277:                 DMA_read_memory (nchan, tmpbuf, fdctrl->data_pos, len);
                   1278:                 ret = memcmp(tmpbuf, fdctrl->fifo + rel_pos, len);
                   1279:                 if (ret == 0) {
1.1.1.4 ! root     1280:                     status2 = FD_SR2_SEH;
1.1       root     1281:                     goto end_transfer;
                   1282:                 }
                   1283:                 if ((ret < 0 && fdctrl->data_dir == FD_DIR_SCANL) ||
                   1284:                     (ret > 0 && fdctrl->data_dir == FD_DIR_SCANH)) {
                   1285:                     status2 = 0x00;
                   1286:                     goto end_transfer;
                   1287:                 }
                   1288:             }
1.1.1.3   root     1289:             break;
1.1       root     1290:         }
1.1.1.3   root     1291:         fdctrl->data_pos += len;
                   1292:         rel_pos = fdctrl->data_pos % FD_SECTOR_LEN;
1.1       root     1293:         if (rel_pos == 0) {
                   1294:             /* Seek to next sector */
1.1.1.4 ! root     1295:             if (!fdctrl_seek_to_next_sect(fdctrl, cur_drv))
        !          1296:                 break;
1.1       root     1297:         }
                   1298:     }
1.1.1.3   root     1299:  end_transfer:
1.1       root     1300:     len = fdctrl->data_pos - start_pos;
                   1301:     FLOPPY_DPRINTF("end transfer %d %d %d\n",
1.1.1.3   root     1302:                    fdctrl->data_pos, len, fdctrl->data_len);
1.1       root     1303:     if (fdctrl->data_dir == FD_DIR_SCANE ||
                   1304:         fdctrl->data_dir == FD_DIR_SCANL ||
                   1305:         fdctrl->data_dir == FD_DIR_SCANH)
1.1.1.4 ! root     1306:         status2 = FD_SR2_SEH;
1.1       root     1307:     if (FD_DID_SEEK(fdctrl->data_state))
1.1.1.4 ! root     1308:         status0 |= FD_SR0_SEEK;
1.1       root     1309:     fdctrl->data_len -= len;
                   1310:     fdctrl_stop_transfer(fdctrl, status0, status1, status2);
1.1.1.3   root     1311:  transfer_error:
1.1       root     1312: 
                   1313:     return len;
                   1314: }
                   1315: 
                   1316: /* Data register : 0x05 */
                   1317: static uint32_t fdctrl_read_data (fdctrl_t *fdctrl)
                   1318: {
                   1319:     fdrive_t *cur_drv;
                   1320:     uint32_t retval = 0;
1.1.1.4 ! root     1321:     int pos;
1.1       root     1322: 
                   1323:     cur_drv = get_cur_drv(fdctrl);
1.1.1.4 ! root     1324:     fdctrl->dsr &= ~FD_DSR_PWRDOWN;
        !          1325:     if (!(fdctrl->msr & FD_MSR_RQM) || !(fdctrl->msr & FD_MSR_DIO)) {
        !          1326:         FLOPPY_ERROR("controller not ready for reading\n");
1.1       root     1327:         return 0;
                   1328:     }
                   1329:     pos = fdctrl->data_pos;
1.1.1.4 ! root     1330:     if (fdctrl->msr & FD_MSR_NONDMA) {
1.1       root     1331:         pos %= FD_SECTOR_LEN;
                   1332:         if (pos == 0) {
1.1.1.4 ! root     1333:             if (fdctrl->data_pos != 0)
        !          1334:                 if (!fdctrl_seek_to_next_sect(fdctrl, cur_drv)) {
        !          1335:                     FLOPPY_DPRINTF("error seeking to next sector %d\n",
        !          1336:                                    fd_sector(cur_drv));
        !          1337:                     return 0;
        !          1338:                 }
        !          1339:             if (bdrv_read(cur_drv->bs, fd_sector(cur_drv), fdctrl->fifo, 1) < 0) {
        !          1340:                 FLOPPY_DPRINTF("error getting sector %d\n",
        !          1341:                                fd_sector(cur_drv));
        !          1342:                 /* Sure, image size is too small... */
        !          1343:                 memset(fdctrl->fifo, 0, FD_SECTOR_LEN);
        !          1344:             }
1.1       root     1345:         }
                   1346:     }
                   1347:     retval = fdctrl->fifo[pos];
                   1348:     if (++fdctrl->data_pos == fdctrl->data_len) {
                   1349:         fdctrl->data_pos = 0;
                   1350:         /* Switch from transfer mode to status mode
                   1351:          * then from status mode to command mode
                   1352:          */
1.1.1.4 ! root     1353:         if (fdctrl->msr & FD_MSR_NONDMA) {
        !          1354:             fdctrl_stop_transfer(fdctrl, FD_SR0_SEEK, 0x00, 0x00);
1.1       root     1355:         } else {
                   1356:             fdctrl_reset_fifo(fdctrl);
                   1357:             fdctrl_reset_irq(fdctrl);
                   1358:         }
                   1359:     }
                   1360:     FLOPPY_DPRINTF("data register: 0x%02x\n", retval);
                   1361: 
                   1362:     return retval;
                   1363: }
                   1364: 
                   1365: static void fdctrl_format_sector (fdctrl_t *fdctrl)
                   1366: {
                   1367:     fdrive_t *cur_drv;
                   1368:     uint8_t kh, kt, ks;
                   1369: 
1.1.1.4 ! root     1370:     SET_CUR_DRV(fdctrl, fdctrl->fifo[1] & FD_DOR_SELMASK);
1.1       root     1371:     cur_drv = get_cur_drv(fdctrl);
                   1372:     kt = fdctrl->fifo[6];
                   1373:     kh = fdctrl->fifo[7];
                   1374:     ks = fdctrl->fifo[8];
                   1375:     FLOPPY_DPRINTF("format sector at %d %d %02x %02x (%d)\n",
1.1.1.4 ! root     1376:                    GET_CUR_DRV(fdctrl), kh, kt, ks,
1.1       root     1377:                    _fd_sector(kh, kt, ks, cur_drv->last_sect));
1.1.1.4 ! root     1378:     switch (fd_seek(cur_drv, kh, kt, ks, fdctrl->config & FD_CONFIG_EIS)) {
1.1       root     1379:     case 2:
                   1380:         /* sect too big */
1.1.1.4 ! root     1381:         fdctrl_stop_transfer(fdctrl, FD_SR0_ABNTERM, 0x00, 0x00);
1.1       root     1382:         fdctrl->fifo[3] = kt;
                   1383:         fdctrl->fifo[4] = kh;
                   1384:         fdctrl->fifo[5] = ks;
                   1385:         return;
                   1386:     case 3:
                   1387:         /* track too big */
1.1.1.4 ! root     1388:         fdctrl_stop_transfer(fdctrl, FD_SR0_ABNTERM, FD_SR1_EC, 0x00);
1.1       root     1389:         fdctrl->fifo[3] = kt;
                   1390:         fdctrl->fifo[4] = kh;
                   1391:         fdctrl->fifo[5] = ks;
                   1392:         return;
                   1393:     case 4:
                   1394:         /* No seek enabled */
1.1.1.4 ! root     1395:         fdctrl_stop_transfer(fdctrl, FD_SR0_ABNTERM, 0x00, 0x00);
1.1       root     1396:         fdctrl->fifo[3] = kt;
                   1397:         fdctrl->fifo[4] = kh;
                   1398:         fdctrl->fifo[5] = ks;
                   1399:         return;
                   1400:     case 1:
                   1401:         fdctrl->data_state |= FD_STATE_SEEK;
                   1402:         break;
                   1403:     default:
                   1404:         break;
                   1405:     }
                   1406:     memset(fdctrl->fifo, 0, FD_SECTOR_LEN);
                   1407:     if (cur_drv->bs == NULL ||
                   1408:         bdrv_write(cur_drv->bs, fd_sector(cur_drv), fdctrl->fifo, 1) < 0) {
1.1.1.3   root     1409:         FLOPPY_ERROR("formatting sector %d\n", fd_sector(cur_drv));
1.1.1.4 ! root     1410:         fdctrl_stop_transfer(fdctrl, FD_SR0_ABNTERM | FD_SR0_SEEK, 0x00, 0x00);
1.1       root     1411:     } else {
1.1.1.3   root     1412:         if (cur_drv->sect == cur_drv->last_sect) {
                   1413:             fdctrl->data_state &= ~FD_STATE_FORMAT;
                   1414:             /* Last sector done */
                   1415:             if (FD_DID_SEEK(fdctrl->data_state))
1.1.1.4 ! root     1416:                 fdctrl_stop_transfer(fdctrl, FD_SR0_SEEK, 0x00, 0x00);
1.1.1.3   root     1417:             else
                   1418:                 fdctrl_stop_transfer(fdctrl, 0x00, 0x00, 0x00);
                   1419:         } else {
                   1420:             /* More to do */
                   1421:             fdctrl->data_pos = 0;
                   1422:             fdctrl->data_len = 4;
                   1423:         }
1.1       root     1424:     }
                   1425: }
                   1426: 
1.1.1.4 ! root     1427: static void fdctrl_handle_lock (fdctrl_t *fdctrl, int direction)
1.1       root     1428: {
1.1.1.4 ! root     1429:     fdctrl->lock = (fdctrl->fifo[0] & 0x80) ? 1 : 0;
        !          1430:     fdctrl->fifo[0] = fdctrl->lock << 4;
        !          1431:     fdctrl_set_fifo(fdctrl, 1, fdctrl->lock);
        !          1432: }
1.1       root     1433: 
1.1.1.4 ! root     1434: static void fdctrl_handle_dumpreg (fdctrl_t *fdctrl, int direction)
        !          1435: {
        !          1436:     fdrive_t *cur_drv = get_cur_drv(fdctrl);
        !          1437: 
        !          1438:     /* Drives position */
        !          1439:     fdctrl->fifo[0] = drv0(fdctrl)->track;
        !          1440:     fdctrl->fifo[1] = drv1(fdctrl)->track;
        !          1441: #if MAX_FD == 4
        !          1442:     fdctrl->fifo[2] = drv2(fdctrl)->track;
        !          1443:     fdctrl->fifo[3] = drv3(fdctrl)->track;
1.1       root     1444: #else
1.1.1.4 ! root     1445:     fdctrl->fifo[2] = 0;
        !          1446:     fdctrl->fifo[3] = 0;
1.1       root     1447: #endif
1.1.1.4 ! root     1448:     /* timers */
        !          1449:     fdctrl->fifo[4] = fdctrl->timer0;
        !          1450:     fdctrl->fifo[5] = (fdctrl->timer1 << 1) | (fdctrl->dor & FD_DOR_DMAEN ? 1 : 0);
        !          1451:     fdctrl->fifo[6] = cur_drv->last_sect;
        !          1452:     fdctrl->fifo[7] = (fdctrl->lock << 7) |
        !          1453:         (cur_drv->perpendicular << 2);
        !          1454:     fdctrl->fifo[8] = fdctrl->config;
        !          1455:     fdctrl->fifo[9] = fdctrl->precomp_trk;
        !          1456:     fdctrl_set_fifo(fdctrl, 10, 0);
        !          1457: }
        !          1458: 
        !          1459: static void fdctrl_handle_version (fdctrl_t *fdctrl, int direction)
        !          1460: {
        !          1461:     /* Controller's version */
        !          1462:     fdctrl->fifo[0] = fdctrl->version;
        !          1463:     fdctrl_set_fifo(fdctrl, 1, 1);
        !          1464: }
        !          1465: 
        !          1466: static void fdctrl_handle_partid (fdctrl_t *fdctrl, int direction)
        !          1467: {
        !          1468:     fdctrl->fifo[0] = 0x41; /* Stepping 1 */
        !          1469:     fdctrl_set_fifo(fdctrl, 1, 0);
        !          1470: }
        !          1471: 
        !          1472: static void fdctrl_handle_restore (fdctrl_t *fdctrl, int direction)
        !          1473: {
        !          1474:     fdrive_t *cur_drv = get_cur_drv(fdctrl);
        !          1475: 
        !          1476:     /* Drives position */
        !          1477:     drv0(fdctrl)->track = fdctrl->fifo[3];
        !          1478:     drv1(fdctrl)->track = fdctrl->fifo[4];
        !          1479: #if MAX_FD == 4
        !          1480:     drv2(fdctrl)->track = fdctrl->fifo[5];
        !          1481:     drv3(fdctrl)->track = fdctrl->fifo[6];
        !          1482: #endif
        !          1483:     /* timers */
        !          1484:     fdctrl->timer0 = fdctrl->fifo[7];
        !          1485:     fdctrl->timer1 = fdctrl->fifo[8];
        !          1486:     cur_drv->last_sect = fdctrl->fifo[9];
        !          1487:     fdctrl->lock = fdctrl->fifo[10] >> 7;
        !          1488:     cur_drv->perpendicular = (fdctrl->fifo[10] >> 2) & 0xF;
        !          1489:     fdctrl->config = fdctrl->fifo[11];
        !          1490:     fdctrl->precomp_trk = fdctrl->fifo[12];
        !          1491:     fdctrl->pwrd = fdctrl->fifo[13];
        !          1492:     fdctrl_reset_fifo(fdctrl);
        !          1493: }
        !          1494: 
        !          1495: static void fdctrl_handle_save (fdctrl_t *fdctrl, int direction)
        !          1496: {
        !          1497:     fdrive_t *cur_drv = get_cur_drv(fdctrl);
        !          1498: 
        !          1499:     fdctrl->fifo[0] = 0;
        !          1500:     fdctrl->fifo[1] = 0;
        !          1501:     /* Drives position */
        !          1502:     fdctrl->fifo[2] = drv0(fdctrl)->track;
        !          1503:     fdctrl->fifo[3] = drv1(fdctrl)->track;
        !          1504: #if MAX_FD == 4
        !          1505:     fdctrl->fifo[4] = drv2(fdctrl)->track;
        !          1506:     fdctrl->fifo[5] = drv3(fdctrl)->track;
        !          1507: #else
        !          1508:     fdctrl->fifo[4] = 0;
        !          1509:     fdctrl->fifo[5] = 0;
        !          1510: #endif
        !          1511:     /* timers */
        !          1512:     fdctrl->fifo[6] = fdctrl->timer0;
        !          1513:     fdctrl->fifo[7] = fdctrl->timer1;
        !          1514:     fdctrl->fifo[8] = cur_drv->last_sect;
        !          1515:     fdctrl->fifo[9] = (fdctrl->lock << 7) |
        !          1516:         (cur_drv->perpendicular << 2);
        !          1517:     fdctrl->fifo[10] = fdctrl->config;
        !          1518:     fdctrl->fifo[11] = fdctrl->precomp_trk;
        !          1519:     fdctrl->fifo[12] = fdctrl->pwrd;
        !          1520:     fdctrl->fifo[13] = 0;
        !          1521:     fdctrl->fifo[14] = 0;
        !          1522:     fdctrl_set_fifo(fdctrl, 15, 1);
        !          1523: }
        !          1524: 
        !          1525: static void fdctrl_handle_readid (fdctrl_t *fdctrl, int direction)
        !          1526: {
        !          1527:     fdrive_t *cur_drv = get_cur_drv(fdctrl);
        !          1528: 
        !          1529:     /* XXX: should set main status register to busy */
        !          1530:     cur_drv->head = (fdctrl->fifo[1] >> 2) & 1;
        !          1531:     qemu_mod_timer(fdctrl->result_timer,
        !          1532:                    qemu_get_clock(vm_clock) + (ticks_per_sec / 50));
        !          1533: }
        !          1534: 
        !          1535: static void fdctrl_handle_format_track (fdctrl_t *fdctrl, int direction)
        !          1536: {
        !          1537:     fdrive_t *cur_drv;
        !          1538: 
        !          1539:     SET_CUR_DRV(fdctrl, fdctrl->fifo[1] & FD_DOR_SELMASK);
        !          1540:     cur_drv = get_cur_drv(fdctrl);
        !          1541:     fdctrl->data_state |= FD_STATE_FORMAT;
        !          1542:     if (fdctrl->fifo[0] & 0x80)
        !          1543:         fdctrl->data_state |= FD_STATE_MULTI;
        !          1544:     else
        !          1545:         fdctrl->data_state &= ~FD_STATE_MULTI;
        !          1546:     fdctrl->data_state &= ~FD_STATE_SEEK;
        !          1547:     cur_drv->bps =
        !          1548:         fdctrl->fifo[2] > 7 ? 16384 : 128 << fdctrl->fifo[2];
        !          1549: #if 0
        !          1550:     cur_drv->last_sect =
        !          1551:         cur_drv->flags & FDISK_DBL_SIDES ? fdctrl->fifo[3] :
        !          1552:         fdctrl->fifo[3] / 2;
        !          1553: #else
        !          1554:     cur_drv->last_sect = fdctrl->fifo[3];
        !          1555: #endif
        !          1556:     /* TODO: implement format using DMA expected by the Bochs BIOS
        !          1557:      * and Linux fdformat (read 3 bytes per sector via DMA and fill
        !          1558:      * the sector with the specified fill byte
        !          1559:      */
        !          1560:     fdctrl->data_state &= ~FD_STATE_FORMAT;
        !          1561:     fdctrl_stop_transfer(fdctrl, 0x00, 0x00, 0x00);
        !          1562: }
        !          1563: 
        !          1564: static void fdctrl_handle_specify (fdctrl_t *fdctrl, int direction)
        !          1565: {
        !          1566:     fdctrl->timer0 = (fdctrl->fifo[1] >> 4) & 0xF;
        !          1567:     fdctrl->timer1 = fdctrl->fifo[2] >> 1;
        !          1568:     if (fdctrl->fifo[2] & 1)
        !          1569:         fdctrl->dor &= ~FD_DOR_DMAEN;
        !          1570:     else
        !          1571:         fdctrl->dor |= FD_DOR_DMAEN;
        !          1572:     /* No result back */
        !          1573:     fdctrl_reset_fifo(fdctrl);
        !          1574: }
        !          1575: 
        !          1576: static void fdctrl_handle_sense_drive_status (fdctrl_t *fdctrl, int direction)
        !          1577: {
        !          1578:     fdrive_t *cur_drv;
        !          1579: 
        !          1580:     SET_CUR_DRV(fdctrl, fdctrl->fifo[1] & FD_DOR_SELMASK);
        !          1581:     cur_drv = get_cur_drv(fdctrl);
        !          1582:     cur_drv->head = (fdctrl->fifo[1] >> 2) & 1;
        !          1583:     /* 1 Byte status back */
        !          1584:     fdctrl->fifo[0] = (cur_drv->ro << 6) |
        !          1585:         (cur_drv->track == 0 ? 0x10 : 0x00) |
        !          1586:         (cur_drv->head << 2) |
        !          1587:         GET_CUR_DRV(fdctrl) |
        !          1588:         0x28;
        !          1589:     fdctrl_set_fifo(fdctrl, 1, 0);
        !          1590: }
        !          1591: 
        !          1592: static void fdctrl_handle_recalibrate (fdctrl_t *fdctrl, int direction)
        !          1593: {
        !          1594:     fdrive_t *cur_drv;
        !          1595: 
        !          1596:     SET_CUR_DRV(fdctrl, fdctrl->fifo[1] & FD_DOR_SELMASK);
        !          1597:     cur_drv = get_cur_drv(fdctrl);
        !          1598:     fd_recalibrate(cur_drv);
        !          1599:     fdctrl_reset_fifo(fdctrl);
        !          1600:     /* Raise Interrupt */
        !          1601:     fdctrl_raise_irq(fdctrl, FD_SR0_SEEK);
        !          1602: }
        !          1603: 
        !          1604: static void fdctrl_handle_sense_interrupt_status (fdctrl_t *fdctrl, int direction)
        !          1605: {
        !          1606:     fdrive_t *cur_drv = get_cur_drv(fdctrl);
        !          1607: 
        !          1608:     if(fdctrl->reset_sensei > 0) {
        !          1609:         fdctrl->fifo[0] =
        !          1610:             FD_SR0_RDYCHG + FD_RESET_SENSEI_COUNT - fdctrl->reset_sensei;
        !          1611:         fdctrl->reset_sensei--;
        !          1612:     } else {
        !          1613:         /* XXX: status0 handling is broken for read/write
        !          1614:            commands, so we do this hack. It should be suppressed
        !          1615:            ASAP */
        !          1616:         fdctrl->fifo[0] =
        !          1617:             FD_SR0_SEEK | (cur_drv->head << 2) | GET_CUR_DRV(fdctrl);
        !          1618:     }
        !          1619: 
        !          1620:     fdctrl->fifo[1] = cur_drv->track;
        !          1621:     fdctrl_set_fifo(fdctrl, 2, 0);
        !          1622:     fdctrl_reset_irq(fdctrl);
        !          1623:     fdctrl->status0 = FD_SR0_RDYCHG;
        !          1624: }
        !          1625: 
        !          1626: static void fdctrl_handle_seek (fdctrl_t *fdctrl, int direction)
        !          1627: {
        !          1628:     fdrive_t *cur_drv;
        !          1629: 
        !          1630:     SET_CUR_DRV(fdctrl, fdctrl->fifo[1] & FD_DOR_SELMASK);
        !          1631:     cur_drv = get_cur_drv(fdctrl);
        !          1632:     fdctrl_reset_fifo(fdctrl);
        !          1633:     if (fdctrl->fifo[2] > cur_drv->max_track) {
        !          1634:         fdctrl_raise_irq(fdctrl, FD_SR0_ABNTERM | FD_SR0_SEEK);
        !          1635:     } else {
        !          1636:         cur_drv->track = fdctrl->fifo[2];
        !          1637:         /* Raise Interrupt */
        !          1638:         fdctrl_raise_irq(fdctrl, FD_SR0_SEEK);
        !          1639:     }
        !          1640: }
        !          1641: 
        !          1642: static void fdctrl_handle_perpendicular_mode (fdctrl_t *fdctrl, int direction)
        !          1643: {
        !          1644:     fdrive_t *cur_drv = get_cur_drv(fdctrl);
        !          1645: 
        !          1646:     if (fdctrl->fifo[1] & 0x80)
        !          1647:         cur_drv->perpendicular = fdctrl->fifo[1] & 0x7;
        !          1648:     /* No result back */
        !          1649:     fdctrl_reset_fifo(fdctrl);
        !          1650: }
        !          1651: 
        !          1652: static void fdctrl_handle_configure (fdctrl_t *fdctrl, int direction)
        !          1653: {
        !          1654:     fdctrl->config = fdctrl->fifo[2];
        !          1655:     fdctrl->precomp_trk =  fdctrl->fifo[3];
        !          1656:     /* No result back */
        !          1657:     fdctrl_reset_fifo(fdctrl);
        !          1658: }
        !          1659: 
        !          1660: static void fdctrl_handle_powerdown_mode (fdctrl_t *fdctrl, int direction)
        !          1661: {
        !          1662:     fdctrl->pwrd = fdctrl->fifo[1];
        !          1663:     fdctrl->fifo[0] = fdctrl->fifo[1];
        !          1664:     fdctrl_set_fifo(fdctrl, 1, 1);
        !          1665: }
        !          1666: 
        !          1667: static void fdctrl_handle_option (fdctrl_t *fdctrl, int direction)
        !          1668: {
        !          1669:     /* No result back */
        !          1670:     fdctrl_reset_fifo(fdctrl);
        !          1671: }
        !          1672: 
        !          1673: static void fdctrl_handle_drive_specification_command (fdctrl_t *fdctrl, int direction)
        !          1674: {
        !          1675:     fdrive_t *cur_drv = get_cur_drv(fdctrl);
        !          1676: 
        !          1677:     if (fdctrl->fifo[fdctrl->data_pos - 1] & 0x80) {
        !          1678:         /* Command parameters done */
        !          1679:         if (fdctrl->fifo[fdctrl->data_pos - 1] & 0x40) {
        !          1680:             fdctrl->fifo[0] = fdctrl->fifo[1];
        !          1681:             fdctrl->fifo[2] = 0;
        !          1682:             fdctrl->fifo[3] = 0;
        !          1683:             fdctrl_set_fifo(fdctrl, 4, 1);
        !          1684:         } else {
        !          1685:             fdctrl_reset_fifo(fdctrl);
1.1       root     1686:         }
1.1.1.4 ! root     1687:     } else if (fdctrl->data_len > 7) {
        !          1688:         /* ERROR */
        !          1689:         fdctrl->fifo[0] = 0x80 |
        !          1690:             (cur_drv->head << 2) | GET_CUR_DRV(fdctrl);
        !          1691:         fdctrl_set_fifo(fdctrl, 1, 1);
        !          1692:     }
        !          1693: }
        !          1694: 
        !          1695: static void fdctrl_handle_relative_seek_out (fdctrl_t *fdctrl, int direction)
        !          1696: {
        !          1697:     fdrive_t *cur_drv;
        !          1698: 
        !          1699:     SET_CUR_DRV(fdctrl, fdctrl->fifo[1] & FD_DOR_SELMASK);
        !          1700:     cur_drv = get_cur_drv(fdctrl);
        !          1701:     if (fdctrl->fifo[2] + cur_drv->track >= cur_drv->max_track) {
        !          1702:         cur_drv->track = cur_drv->max_track - 1;
        !          1703:     } else {
        !          1704:         cur_drv->track += fdctrl->fifo[2];
1.1       root     1705:     }
1.1.1.4 ! root     1706:     fdctrl_reset_fifo(fdctrl);
        !          1707:     /* Raise Interrupt */
        !          1708:     fdctrl_raise_irq(fdctrl, FD_SR0_SEEK);
        !          1709: }
        !          1710: 
        !          1711: static void fdctrl_handle_relative_seek_in (fdctrl_t *fdctrl, int direction)
        !          1712: {
        !          1713:     fdrive_t *cur_drv;
        !          1714: 
        !          1715:     SET_CUR_DRV(fdctrl, fdctrl->fifo[1] & FD_DOR_SELMASK);
        !          1716:     cur_drv = get_cur_drv(fdctrl);
        !          1717:     if (fdctrl->fifo[2] > cur_drv->track) {
        !          1718:         cur_drv->track = 0;
        !          1719:     } else {
        !          1720:         cur_drv->track -= fdctrl->fifo[2];
        !          1721:     }
        !          1722:     fdctrl_reset_fifo(fdctrl);
        !          1723:     /* Raise Interrupt */
        !          1724:     fdctrl_raise_irq(fdctrl, FD_SR0_SEEK);
        !          1725: }
        !          1726: 
        !          1727: static const struct {
        !          1728:     uint8_t value;
        !          1729:     uint8_t mask;
        !          1730:     const char* name;
        !          1731:     int parameters;
        !          1732:     void (*handler)(fdctrl_t *fdctrl, int direction);
        !          1733:     int direction;
        !          1734: } handlers[] = {
        !          1735:     { FD_CMD_READ, 0x1f, "READ", 8, fdctrl_start_transfer, FD_DIR_READ },
        !          1736:     { FD_CMD_WRITE, 0x3f, "WRITE", 8, fdctrl_start_transfer, FD_DIR_WRITE },
        !          1737:     { FD_CMD_SEEK, 0xff, "SEEK", 2, fdctrl_handle_seek },
        !          1738:     { FD_CMD_SENSE_INTERRUPT_STATUS, 0xff, "SENSE INTERRUPT STATUS", 0, fdctrl_handle_sense_interrupt_status },
        !          1739:     { FD_CMD_RECALIBRATE, 0xff, "RECALIBRATE", 1, fdctrl_handle_recalibrate },
        !          1740:     { FD_CMD_FORMAT_TRACK, 0xbf, "FORMAT TRACK", 5, fdctrl_handle_format_track },
        !          1741:     { FD_CMD_READ_TRACK, 0xbf, "READ TRACK", 8, fdctrl_start_transfer, FD_DIR_READ },
        !          1742:     { FD_CMD_RESTORE, 0xff, "RESTORE", 17, fdctrl_handle_restore }, /* part of READ DELETED DATA */
        !          1743:     { FD_CMD_SAVE, 0xff, "SAVE", 0, fdctrl_handle_save }, /* part of READ DELETED DATA */
        !          1744:     { FD_CMD_READ_DELETED, 0x1f, "READ DELETED DATA", 8, fdctrl_start_transfer_del, FD_DIR_READ },
        !          1745:     { FD_CMD_SCAN_EQUAL, 0x1f, "SCAN EQUAL", 8, fdctrl_start_transfer, FD_DIR_SCANE },
        !          1746:     { FD_CMD_VERIFY, 0x1f, "VERIFY", 8, fdctrl_unimplemented },
        !          1747:     { FD_CMD_SCAN_LOW_OR_EQUAL, 0x1f, "SCAN LOW OR EQUAL", 8, fdctrl_start_transfer, FD_DIR_SCANL },
        !          1748:     { FD_CMD_SCAN_HIGH_OR_EQUAL, 0x1f, "SCAN HIGH OR EQUAL", 8, fdctrl_start_transfer, FD_DIR_SCANH },
        !          1749:     { FD_CMD_WRITE_DELETED, 0x3f, "WRITE DELETED DATA", 8, fdctrl_start_transfer_del, FD_DIR_WRITE },
        !          1750:     { FD_CMD_READ_ID, 0xbf, "READ ID", 1, fdctrl_handle_readid },
        !          1751:     { FD_CMD_SPECIFY, 0xff, "SPECIFY", 2, fdctrl_handle_specify },
        !          1752:     { FD_CMD_SENSE_DRIVE_STATUS, 0xff, "SENSE DRIVE STATUS", 1, fdctrl_handle_sense_drive_status },
        !          1753:     { FD_CMD_PERPENDICULAR_MODE, 0xff, "PERPENDICULAR MODE", 1, fdctrl_handle_perpendicular_mode },
        !          1754:     { FD_CMD_CONFIGURE, 0xff, "CONFIGURE", 3, fdctrl_handle_configure },
        !          1755:     { FD_CMD_POWERDOWN_MODE, 0xff, "POWERDOWN MODE", 2, fdctrl_handle_powerdown_mode },
        !          1756:     { FD_CMD_OPTION, 0xff, "OPTION", 1, fdctrl_handle_option },
        !          1757:     { FD_CMD_DRIVE_SPECIFICATION_COMMAND, 0xff, "DRIVE SPECIFICATION COMMAND", 5, fdctrl_handle_drive_specification_command },
        !          1758:     { FD_CMD_RELATIVE_SEEK_OUT, 0xff, "RELATIVE SEEK OUT", 2, fdctrl_handle_relative_seek_out },
        !          1759:     { FD_CMD_FORMAT_AND_WRITE, 0xff, "FORMAT AND WRITE", 10, fdctrl_unimplemented },
        !          1760:     { FD_CMD_RELATIVE_SEEK_IN, 0xff, "RELATIVE SEEK IN", 2, fdctrl_handle_relative_seek_in },
        !          1761:     { FD_CMD_LOCK, 0x7f, "LOCK", 0, fdctrl_handle_lock },
        !          1762:     { FD_CMD_DUMPREG, 0xff, "DUMPREG", 0, fdctrl_handle_dumpreg },
        !          1763:     { FD_CMD_VERSION, 0xff, "VERSION", 0, fdctrl_handle_version },
        !          1764:     { FD_CMD_PART_ID, 0xff, "PART ID", 0, fdctrl_handle_partid },
        !          1765:     { FD_CMD_WRITE, 0x1f, "WRITE (BeOS)", 8, fdctrl_start_transfer, FD_DIR_WRITE }, /* not in specification ; BeOS 4.5 bug */
        !          1766:     { 0, 0, "unknown", 0, fdctrl_unimplemented }, /* default handler */
        !          1767: };
        !          1768: /* Associate command to an index in the 'handlers' array */
        !          1769: static uint8_t command_to_handler[256];
        !          1770: 
        !          1771: static void fdctrl_write_data (fdctrl_t *fdctrl, uint32_t value)
        !          1772: {
        !          1773:     fdrive_t *cur_drv;
        !          1774:     int pos;
        !          1775: 
        !          1776:     /* Reset mode */
        !          1777:     if (!(fdctrl->dor & FD_DOR_nRESET)) {
        !          1778:         FLOPPY_DPRINTF("Floppy controller in RESET state !\n");
        !          1779:         return;
        !          1780:     }
        !          1781:     if (!(fdctrl->msr & FD_MSR_RQM) || (fdctrl->msr & FD_MSR_DIO)) {
        !          1782:         FLOPPY_ERROR("controller not ready for writing\n");
        !          1783:         return;
        !          1784:     }
        !          1785:     fdctrl->dsr &= ~FD_DSR_PWRDOWN;
        !          1786:     /* Is it write command time ? */
        !          1787:     if (fdctrl->msr & FD_MSR_NONDMA) {
        !          1788:         /* FIFO data write */
        !          1789:         pos = fdctrl->data_pos++;
        !          1790:         pos %= FD_SECTOR_LEN;
        !          1791:         fdctrl->fifo[pos] = value;
        !          1792:         if (pos == FD_SECTOR_LEN - 1 ||
        !          1793:             fdctrl->data_pos == fdctrl->data_len) {
        !          1794:             cur_drv = get_cur_drv(fdctrl);
        !          1795:             if (bdrv_write(cur_drv->bs, fd_sector(cur_drv), fdctrl->fifo, 1) < 0) {
        !          1796:                 FLOPPY_ERROR("writing sector %d\n", fd_sector(cur_drv));
        !          1797:                 return;
        !          1798:             }
        !          1799:             if (!fdctrl_seek_to_next_sect(fdctrl, cur_drv)) {
        !          1800:                 FLOPPY_DPRINTF("error seeking to next sector %d\n",
        !          1801:                                fd_sector(cur_drv));
        !          1802:                 return;
        !          1803:             }
        !          1804:         }
        !          1805:         /* Switch from transfer mode to status mode
        !          1806:          * then from status mode to command mode
        !          1807:          */
        !          1808:         if (fdctrl->data_pos == fdctrl->data_len)
        !          1809:             fdctrl_stop_transfer(fdctrl, FD_SR0_SEEK, 0x00, 0x00);
        !          1810:         return;
        !          1811:     }
        !          1812:     if (fdctrl->data_pos == 0) {
        !          1813:         /* Command */
        !          1814:         pos = command_to_handler[value & 0xff];
        !          1815:         FLOPPY_DPRINTF("%s command\n", handlers[pos].name);
        !          1816:         fdctrl->data_len = handlers[pos].parameters + 1;
        !          1817:     }
        !          1818: 
1.1       root     1819:     FLOPPY_DPRINTF("%s: %02x\n", __func__, value);
1.1.1.4 ! root     1820:     fdctrl->fifo[fdctrl->data_pos++] = value;
        !          1821:     if (fdctrl->data_pos == fdctrl->data_len) {
1.1       root     1822:         /* We now have all parameters
                   1823:          * and will be able to treat the command
                   1824:          */
1.1.1.3   root     1825:         if (fdctrl->data_state & FD_STATE_FORMAT) {
                   1826:             fdctrl_format_sector(fdctrl);
1.1       root     1827:             return;
                   1828:         }
1.1.1.4 ! root     1829: 
        !          1830:         pos = command_to_handler[fdctrl->fifo[0] & 0xff];
        !          1831:         FLOPPY_DPRINTF("treat %s command\n", handlers[pos].name);
        !          1832:         (*handlers[pos].handler)(fdctrl, handlers[pos].direction);
1.1       root     1833:     }
                   1834: }
                   1835: 
                   1836: static void fdctrl_result_timer(void *opaque)
                   1837: {
                   1838:     fdctrl_t *fdctrl = opaque;
1.1.1.3   root     1839:     fdrive_t *cur_drv = get_cur_drv(fdctrl);
                   1840: 
                   1841:     /* Pretend we are spinning.
                   1842:      * This is needed for Coherent, which uses READ ID to check for
                   1843:      * sector interleaving.
                   1844:      */
                   1845:     if (cur_drv->last_sect != 0) {
                   1846:         cur_drv->sect = (cur_drv->sect % cur_drv->last_sect) + 1;
                   1847:     }
1.1       root     1848:     fdctrl_stop_transfer(fdctrl, 0x00, 0x00, 0x00);
                   1849: }
1.1.1.4 ! root     1850: 
        !          1851: /* Init functions */
        !          1852: static fdctrl_t *fdctrl_init_common (qemu_irq irq, int dma_chann,
        !          1853:                                      target_phys_addr_t io_base,
        !          1854:                                      BlockDriverState **fds)
        !          1855: {
        !          1856:     fdctrl_t *fdctrl;
        !          1857:     int i, j;
        !          1858: 
        !          1859:     /* Fill 'command_to_handler' lookup table */
        !          1860:     for (i = ARRAY_SIZE(handlers) - 1; i >= 0; i--) {
        !          1861:         for (j = 0; j < sizeof(command_to_handler); j++) {
        !          1862:             if ((j & handlers[i].mask) == handlers[i].value)
        !          1863:                 command_to_handler[j] = i;
        !          1864:         }
        !          1865:     }
        !          1866: 
        !          1867:     FLOPPY_DPRINTF("init controller\n");
        !          1868:     fdctrl = qemu_mallocz(sizeof(fdctrl_t));
        !          1869:     fdctrl->fifo = qemu_memalign(512, FD_SECTOR_LEN);
        !          1870:     fdctrl->result_timer = qemu_new_timer(vm_clock,
        !          1871:                                           fdctrl_result_timer, fdctrl);
        !          1872: 
        !          1873:     fdctrl->version = 0x90; /* Intel 82078 controller */
        !          1874:     fdctrl->irq = irq;
        !          1875:     fdctrl->dma_chann = dma_chann;
        !          1876:     fdctrl->io_base = io_base;
        !          1877:     fdctrl->config = FD_CONFIG_EIS | FD_CONFIG_EFIFO; /* Implicit seek, polling & FIFO enabled */
        !          1878:     if (fdctrl->dma_chann != -1) {
        !          1879:         DMA_register_channel(dma_chann, &fdctrl_transfer_handler, fdctrl);
        !          1880:     }
        !          1881:     for (i = 0; i < MAX_FD; i++) {
        !          1882:         fd_init(&fdctrl->drives[i], fds[i]);
        !          1883:     }
        !          1884:     fdctrl_external_reset(fdctrl);
        !          1885:     register_savevm("fdc", io_base, 2, fdc_save, fdc_load, fdctrl);
        !          1886:     qemu_register_reset(fdctrl_external_reset, fdctrl);
        !          1887:     for (i = 0; i < MAX_FD; i++) {
        !          1888:         fd_revalidate(&fdctrl->drives[i]);
        !          1889:     }
        !          1890: 
        !          1891:     return fdctrl;
        !          1892: }
        !          1893: 
        !          1894: fdctrl_t *fdctrl_init (qemu_irq irq, int dma_chann, int mem_mapped,
        !          1895:                        target_phys_addr_t io_base,
        !          1896:                        BlockDriverState **fds)
        !          1897: {
        !          1898:     fdctrl_t *fdctrl;
        !          1899:     int io_mem;
        !          1900: 
        !          1901:     fdctrl = fdctrl_init_common(irq, dma_chann, io_base, fds);
        !          1902: 
        !          1903:     fdctrl->sun4m = 0;
        !          1904:     if (mem_mapped) {
        !          1905:         io_mem = cpu_register_io_memory(0, fdctrl_mem_read, fdctrl_mem_write,
        !          1906:                                         fdctrl);
        !          1907:         cpu_register_physical_memory(io_base, 0x08, io_mem);
        !          1908:     } else {
        !          1909:         register_ioport_read((uint32_t)io_base + 0x01, 5, 1,
        !          1910:                              &fdctrl_read_port, fdctrl);
        !          1911:         register_ioport_read((uint32_t)io_base + 0x07, 1, 1,
        !          1912:                              &fdctrl_read_port, fdctrl);
        !          1913:         register_ioport_write((uint32_t)io_base + 0x01, 5, 1,
        !          1914:                               &fdctrl_write_port, fdctrl);
        !          1915:         register_ioport_write((uint32_t)io_base + 0x07, 1, 1,
        !          1916:                               &fdctrl_write_port, fdctrl);
        !          1917:     }
        !          1918: 
        !          1919:     return fdctrl;
        !          1920: }
        !          1921: 
        !          1922: fdctrl_t *sun4m_fdctrl_init (qemu_irq irq, target_phys_addr_t io_base,
        !          1923:                              BlockDriverState **fds, qemu_irq *fdc_tc)
        !          1924: {
        !          1925:     fdctrl_t *fdctrl;
        !          1926:     int io_mem;
        !          1927: 
        !          1928:     fdctrl = fdctrl_init_common(irq, -1, io_base, fds);
        !          1929:     fdctrl->sun4m = 1;
        !          1930:     io_mem = cpu_register_io_memory(0, fdctrl_mem_read_strict,
        !          1931:                                     fdctrl_mem_write_strict,
        !          1932:                                     fdctrl);
        !          1933:     cpu_register_physical_memory(io_base, 0x08, io_mem);
        !          1934:     *fdc_tc = *qemu_allocate_irqs(fdctrl_handle_tc, fdctrl, 1);
        !          1935: 
        !          1936:     return fdctrl;
        !          1937: }

unix.superglobalmegacorp.com