← Back to branch summary

~vcs-imports/qemu/git

« back to all changes in this revision

Viewing changes to hw/fdc.c

  • Committer: blueswir1
  • Date: 2007-09-25 17:30:09 UTC
  • Revision ID: git-v1:eb296a0a03eebd2d73718d31cd0d8a8db0b804de
 Remove the target dependency introduced by previous patch


git-svn-id: svn://svn.savannah.nongnu.org/qemu/trunk@3239 c046a42c-6fe2-441c-8c8c-71466251a162

Show diffs side-by-side

added added

removed removed

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