Blame view

hw/fdc.c 60.4 KB
1
/*
2
 * QEMU Floppy disk emulator (Intel 82078)
3
 *
4
 * Copyright (c) 2003, 2007 Jocelyn Mayer
5
 *
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
 * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 */
bellard authored
24
25
26
27
/*
 * The controller is used in Sun4m systems in a slightly different
 * way. There are changes in DOR register and DMA is not available.
 */
pbrook authored
28
29
30
31
32
#include "hw.h"
#include "fdc.h"
#include "block.h"
#include "qemu-timer.h"
#include "isa.h"
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59

/********************************************************/
/* debug Floppy devices */
//#define DEBUG_FLOPPY

#ifdef DEBUG_FLOPPY
#define FLOPPY_DPRINTF(fmt, args...) \
do { printf("FLOPPY: " fmt , ##args); } while (0)
#else
#define FLOPPY_DPRINTF(fmt, args...)
#endif

#define FLOPPY_ERROR(fmt, args...) \
do { printf("FLOPPY ERROR: %s: " fmt, __func__ , ##args); } while (0)

/********************************************************/
/* Floppy drive emulation                               */

/* Will always be a fixed parameter for us */
#define FD_SECTOR_LEN 512
#define FD_SECTOR_SC  2   /* Sector size code */

/* Floppy disk drive emulation */
typedef enum fdisk_type_t {
    FDRIVE_DISK_288   = 0x01, /* 2.88 MB disk           */
    FDRIVE_DISK_144   = 0x02, /* 1.44 MB disk           */
    FDRIVE_DISK_720   = 0x03, /* 720 kB disk            */
60
61
    FDRIVE_DISK_USER  = 0x04, /* User defined geometry  */
    FDRIVE_DISK_NONE  = 0x05, /* No disk                */
62
63
64
65
66
67
68
69
70
} fdisk_type_t;

typedef enum fdrive_type_t {
    FDRIVE_DRV_144  = 0x00,   /* 1.44 MB 3"5 drive      */
    FDRIVE_DRV_288  = 0x01,   /* 2.88 MB 3"5 drive      */
    FDRIVE_DRV_120  = 0x02,   /* 1.2  MB 5"25 drive     */
    FDRIVE_DRV_NONE = 0x03,   /* No drive connected     */
} fdrive_type_t;
71
72
73
74
75
76
77
78
typedef enum fdrive_flags_t {
    FDRIVE_MOTOR_ON   = 0x01, /* motor on/off           */
} fdrive_flags_t;

typedef enum fdisk_flags_t {
    FDISK_DBL_SIDES  = 0x01,
} fdisk_flags_t;
79
80
81
82
typedef struct fdrive_t {
    BlockDriverState *bs;
    /* Drive status */
    fdrive_type_t drive;
83
    fdrive_flags_t drflags;
84
85
86
87
88
89
90
91
92
    uint8_t perpendicular;    /* 2.88 MB access mode    */
    /* Position */
    uint8_t head;
    uint8_t track;
    uint8_t sect;
    /* Last operation status */
    uint8_t dir;              /* Direction              */
    uint8_t rw;               /* Read/write             */
    /* Media */
93
    fdisk_flags_t flags;
94
95
    uint8_t last_sect;        /* Nb sector per track    */
    uint8_t max_track;        /* Nb of tracks           */
96
    uint16_t bps;             /* Bytes per sector       */
97
98
99
    uint8_t ro;               /* Is read-only           */
} fdrive_t;
100
static void fd_init (fdrive_t *drv, BlockDriverState *bs)
101
102
{
    /* Drive */
103
    drv->bs = bs;
bellard authored
104
    drv->drive = FDRIVE_DRV_NONE;
105
    drv->drflags = 0;
106
107
    drv->perpendicular = 0;
    /* Disk */
108
    drv->last_sect = 0;
109
110
111
112
    drv->max_track = 0;
}

static int _fd_sector (uint8_t head, uint8_t track,
113
                       uint8_t sect, uint8_t last_sect)
114
115
116
117
118
119
120
121
122
123
124
125
126
127
{
    return (((track * 2) + head) * last_sect) + sect - 1;
}

/* Returns current position, in sectors, for given drive */
static int fd_sector (fdrive_t *drv)
{
    return _fd_sector(drv->head, drv->track, drv->sect, drv->last_sect);
}

static int fd_seek (fdrive_t *drv, uint8_t head, uint8_t track, uint8_t sect,
                    int enable_seek)
{
    uint32_t sector;
128
129
130
    int ret;

    if (track > drv->max_track ||
131
        (head != 0 && (drv->flags & FDISK_DBL_SIDES) == 0)) {
132
133
134
135
        FLOPPY_DPRINTF("try to read %d %02x %02x (max=%d %d %02x %02x)\n",
                       head, track, sect, 1,
                       (drv->flags & FDISK_DBL_SIDES) == 0 ? 0 : 1,
                       drv->max_track, drv->last_sect);
136
137
138
        return 2;
    }
    if (sect > drv->last_sect) {
139
140
141
142
        FLOPPY_DPRINTF("try to read %d %02x %02x (max=%d %d %02x %02x)\n",
                       head, track, sect, 1,
                       (drv->flags & FDISK_DBL_SIDES) == 0 ? 0 : 1,
                       drv->max_track, drv->last_sect);
143
144
145
        return 3;
    }
    sector = _fd_sector(head, track, sect, drv->last_sect);
146
    ret = 0;
147
148
149
150
151
152
153
154
155
    if (sector != fd_sector(drv)) {
#if 0
        if (!enable_seek) {
            FLOPPY_ERROR("no implicit seek %d %02x %02x (max=%d %02x %02x)\n",
                         head, track, sect, 1, drv->max_track, drv->last_sect);
            return 4;
        }
#endif
        drv->head = head;
156
157
        if (drv->track != track)
            ret = 1;
158
159
160
161
        drv->track = track;
        drv->sect = sect;
    }
162
    return ret;
163
164
165
166
167
168
169
170
171
172
173
174
175
}

/* Set drive back to track 0 */
static void fd_recalibrate (fdrive_t *drv)
{
    FLOPPY_DPRINTF("recalibrate\n");
    drv->head = 0;
    drv->track = 0;
    drv->sect = 1;
    drv->dir = 1;
    drv->rw = 0;
}
176
177
178
179
180
181
182
/* Recognize floppy formats */
typedef struct fd_format_t {
    fdrive_type_t drive;
    fdisk_type_t  disk;
    uint8_t last_sect;
    uint8_t max_track;
    uint8_t max_head;
183
    const char *str;
184
185
} fd_format_t;
blueswir1 authored
186
static const fd_format_t fd_formats[] = {
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
    /* First entry is default format */
    /* 1.44 MB 3"1/2 floppy disks */
    { FDRIVE_DRV_144, FDRIVE_DISK_144, 18, 80, 1, "1.44 MB 3\"1/2", },
    { FDRIVE_DRV_144, FDRIVE_DISK_144, 20, 80, 1,  "1.6 MB 3\"1/2", },
    { FDRIVE_DRV_144, FDRIVE_DISK_144, 21, 80, 1, "1.68 MB 3\"1/2", },
    { FDRIVE_DRV_144, FDRIVE_DISK_144, 21, 82, 1, "1.72 MB 3\"1/2", },
    { FDRIVE_DRV_144, FDRIVE_DISK_144, 21, 83, 1, "1.74 MB 3\"1/2", },
    { FDRIVE_DRV_144, FDRIVE_DISK_144, 22, 80, 1, "1.76 MB 3\"1/2", },
    { FDRIVE_DRV_144, FDRIVE_DISK_144, 23, 80, 1, "1.84 MB 3\"1/2", },
    { FDRIVE_DRV_144, FDRIVE_DISK_144, 24, 80, 1, "1.92 MB 3\"1/2", },
    /* 2.88 MB 3"1/2 floppy disks */
    { FDRIVE_DRV_288, FDRIVE_DISK_288, 36, 80, 1, "2.88 MB 3\"1/2", },
    { FDRIVE_DRV_288, FDRIVE_DISK_288, 39, 80, 1, "3.12 MB 3\"1/2", },
    { FDRIVE_DRV_288, FDRIVE_DISK_288, 40, 80, 1,  "3.2 MB 3\"1/2", },
    { FDRIVE_DRV_288, FDRIVE_DISK_288, 44, 80, 1, "3.52 MB 3\"1/2", },
    { FDRIVE_DRV_288, FDRIVE_DISK_288, 48, 80, 1, "3.84 MB 3\"1/2", },
    /* 720 kB 3"1/2 floppy disks */
    { FDRIVE_DRV_144, FDRIVE_DISK_720,  9, 80, 1,  "720 kB 3\"1/2", },
    { FDRIVE_DRV_144, FDRIVE_DISK_720, 10, 80, 1,  "800 kB 3\"1/2", },
    { FDRIVE_DRV_144, FDRIVE_DISK_720, 10, 82, 1,  "820 kB 3\"1/2", },
    { FDRIVE_DRV_144, FDRIVE_DISK_720, 10, 83, 1,  "830 kB 3\"1/2", },
    { FDRIVE_DRV_144, FDRIVE_DISK_720, 13, 80, 1, "1.04 MB 3\"1/2", },
    { FDRIVE_DRV_144, FDRIVE_DISK_720, 14, 80, 1, "1.12 MB 3\"1/2", },
    /* 1.2 MB 5"1/4 floppy disks */
    { FDRIVE_DRV_120, FDRIVE_DISK_288, 15, 80, 1,  "1.2 kB 5\"1/4", },
    { FDRIVE_DRV_120, FDRIVE_DISK_288, 18, 80, 1, "1.44 MB 5\"1/4", },
    { FDRIVE_DRV_120, FDRIVE_DISK_288, 18, 82, 1, "1.48 MB 5\"1/4", },
    { FDRIVE_DRV_120, FDRIVE_DISK_288, 18, 83, 1, "1.49 MB 5\"1/4", },
    { FDRIVE_DRV_120, FDRIVE_DISK_288, 20, 80, 1,  "1.6 MB 5\"1/4", },
    /* 720 kB 5"1/4 floppy disks */
    { FDRIVE_DRV_120, FDRIVE_DISK_288,  9, 80, 1,  "720 kB 5\"1/4", },
    { FDRIVE_DRV_120, FDRIVE_DISK_288, 11, 80, 1,  "880 kB 5\"1/4", },
    /* 360 kB 5"1/4 floppy disks */
    { FDRIVE_DRV_120, FDRIVE_DISK_288,  9, 40, 1,  "360 kB 5\"1/4", },
    { FDRIVE_DRV_120, FDRIVE_DISK_288,  9, 40, 0,  "180 kB 5\"1/4", },
    { FDRIVE_DRV_120, FDRIVE_DISK_288, 10, 41, 1,  "410 kB 5\"1/4", },
    { FDRIVE_DRV_120, FDRIVE_DISK_288, 10, 42, 1,  "420 kB 5\"1/4", },
224
    /* 320 kB 5"1/4 floppy disks */
225
226
227
228
229
230
231
232
    { FDRIVE_DRV_120, FDRIVE_DISK_288,  8, 40, 1,  "320 kB 5\"1/4", },
    { FDRIVE_DRV_120, FDRIVE_DISK_288,  8, 40, 0,  "160 kB 5\"1/4", },
    /* 360 kB must match 5"1/4 better than 3"1/2... */
    { FDRIVE_DRV_144, FDRIVE_DISK_720,  9, 80, 0,  "360 kB 3\"1/2", },
    /* end */
    { FDRIVE_DRV_NONE, FDRIVE_DISK_NONE, -1, -1, 0, NULL, },
};
233
/* Revalidate a disk drive after a disk change */
234
static void fd_revalidate (fdrive_t *drv)
235
{
blueswir1 authored
236
    const fd_format_t *parse;
237
    uint64_t nb_sectors, size;
238
    int i, first_match, match;
239
    int nb_heads, max_track, last_sect, ro;
240
241

    FLOPPY_DPRINTF("revalidate\n");
242
    if (drv->bs != NULL && bdrv_is_inserted(drv->bs)) {
243
244
245
246
        ro = bdrv_is_read_only(drv->bs);
        bdrv_get_geometry_hint(drv->bs, &nb_heads, &max_track, &last_sect);
        if (nb_heads != 0 && max_track != 0 && last_sect != 0) {
            FLOPPY_DPRINTF("User defined disk (%d %d %d)",
247
                           nb_heads - 1, max_track, last_sect);
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
        } else {
            bdrv_get_geometry(drv->bs, &nb_sectors);
            match = -1;
            first_match = -1;
            for (i = 0;; i++) {
                parse = &fd_formats[i];
                if (parse->drive == FDRIVE_DRV_NONE)
                    break;
                if (drv->drive == parse->drive ||
                    drv->drive == FDRIVE_DRV_NONE) {
                    size = (parse->max_head + 1) * parse->max_track *
                        parse->last_sect;
                    if (nb_sectors == size) {
                        match = i;
                        break;
                    }
                    if (first_match == -1)
                        first_match = i;
                }
            }
            if (match == -1) {
                if (first_match == -1)
                    match = 1;
                else
                    match = first_match;
                parse = &fd_formats[match];
            }
            nb_heads = parse->max_head + 1;
            max_track = parse->max_track;
            last_sect = parse->last_sect;
            drv->drive = parse->drive;
            FLOPPY_DPRINTF("%s floppy disk (%d h %d t %d s) %s\n", parse->str,
280
                           nb_heads, max_track, last_sect, ro ? "ro" : "rw");
281
282
283
284
285
286
287
288
289
        }
        if (nb_heads == 1) {
            drv->flags &= ~FDISK_DBL_SIDES;
        } else {
            drv->flags |= FDISK_DBL_SIDES;
        }
        drv->max_track = max_track;
        drv->last_sect = last_sect;
        drv->ro = ro;
290
    } else {
291
        FLOPPY_DPRINTF("No disk in drive\n");
292
        drv->last_sect = 0;
293
294
        drv->max_track = 0;
        drv->flags &= ~FDISK_DBL_SIDES;
295
    }
296
297
}
298
299
300
/* Motor control */
static void fd_start (fdrive_t *drv)
{
301
    drv->drflags |= FDRIVE_MOTOR_ON;
302
303
304
305
}

static void fd_stop (fdrive_t *drv)
{
306
    drv->drflags &= ~FDRIVE_MOTOR_ON;
307
308
309
310
311
312
313
314
315
316
}

/* Re-initialise a drives (motor off, repositioned) */
static void fd_reset (fdrive_t *drv)
{
    fd_stop(drv);
    fd_recalibrate(drv);
}

/********************************************************/
bellard authored
317
/* Intel 82078 floppy disk controller emulation          */
318
319
320
static void fdctrl_reset (fdctrl_t *fdctrl, int do_irq);
static void fdctrl_reset_fifo (fdctrl_t *fdctrl);
bellard authored
321
322
static int fdctrl_transfer_handler (void *opaque, int nchan,
                                    int dma_pos, int dma_len);
323
static void fdctrl_raise_irq (fdctrl_t *fdctrl, uint8_t status);
324
static void fdctrl_result_timer(void *opaque);
325
326
327
328
329
330
331
332
333
334
335

static uint32_t fdctrl_read_statusB (fdctrl_t *fdctrl);
static uint32_t fdctrl_read_dor (fdctrl_t *fdctrl);
static void fdctrl_write_dor (fdctrl_t *fdctrl, uint32_t value);
static uint32_t fdctrl_read_tape (fdctrl_t *fdctrl);
static void fdctrl_write_tape (fdctrl_t *fdctrl, uint32_t value);
static uint32_t fdctrl_read_main_status (fdctrl_t *fdctrl);
static void fdctrl_write_rate (fdctrl_t *fdctrl, uint32_t value);
static uint32_t fdctrl_read_data (fdctrl_t *fdctrl);
static void fdctrl_write_data (fdctrl_t *fdctrl, uint32_t value);
static uint32_t fdctrl_read_dir (fdctrl_t *fdctrl);
336
337

enum {
338
    FD_CTRL_ACTIVE = 0x01, /* XXX: suppress that */
339
    FD_CTRL_RESET  = 0x02,
340
341
    FD_CTRL_SLEEP  = 0x04, /* XXX: suppress that */
    FD_CTRL_BUSY   = 0x08, /* dma transfer in progress */
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
    FD_CTRL_INTR   = 0x10,
};

enum {
    FD_DIR_WRITE   = 0,
    FD_DIR_READ    = 1,
    FD_DIR_SCANE   = 2,
    FD_DIR_SCANL   = 3,
    FD_DIR_SCANH   = 4,
};

enum {
    FD_STATE_CMD    = 0x00,
    FD_STATE_STATUS = 0x01,
    FD_STATE_DATA   = 0x02,
    FD_STATE_STATE  = 0x03,
    FD_STATE_MULTI  = 0x10,
    FD_STATE_SEEK   = 0x20,
360
    FD_STATE_FORMAT = 0x40,
361
362
363
};

#define FD_STATE(state) ((state) & FD_STATE_STATE)
364
365
#define FD_SET_STATE(state, new_state) \
do { (state) = ((state) & ~FD_STATE_STATE) | (new_state); } while (0)
366
367
#define FD_MULTI_TRACK(state) ((state) & FD_STATE_MULTI)
#define FD_DID_SEEK(state) ((state) & FD_STATE_SEEK)
368
#define FD_FORMAT_CMD(state) ((state) & FD_STATE_FORMAT)
369
370
371
struct fdctrl_t {
    fdctrl_t *fdctrl;
bellard authored
372
    /* Controller's identification */
373
374
    uint8_t version;
    /* HW */
pbrook authored
375
    qemu_irq irq;
376
    int dma_chann;
377
    target_phys_addr_t io_base;
bellard authored
378
    /* Controller state */
379
    QEMUTimer *result_timer;
380
381
382
383
384
385
386
387
388
389
390
    uint8_t state;
    uint8_t dma_en;
    uint8_t cur_drv;
    uint8_t bootsel;
    /* Command FIFO */
    uint8_t fifo[FD_SECTOR_LEN];
    uint32_t data_pos;
    uint32_t data_len;
    uint8_t data_state;
    uint8_t data_dir;
    uint8_t int_status;
391
    uint8_t eot; /* last wanted sector */
392
393
394
395
396
397
398
399
400
401
    /* States kept only to be returned back */
    /* Timers state */
    uint8_t timer0;
    uint8_t timer1;
    /* precompensation */
    uint8_t precomp_trk;
    uint8_t config;
    uint8_t lock;
    /* Power down config (also with status regB access mode */
    uint8_t pwrd;
402
    /* Sun4m quirks? */
blueswir1 authored
403
    int sun4m;
404
405
    /* Floppy drives */
    fdrive_t drives[2];
406
407
408
409
410
411
412
};

static uint32_t fdctrl_read (void *opaque, uint32_t reg)
{
    fdctrl_t *fdctrl = opaque;
    uint32_t retval;
413
    switch (reg & 0x07) {
bellard authored
414
    case 0x00:
blueswir1 authored
415
        if (fdctrl->sun4m) {
416
417
418
419
420
            // Identify to Linux as S82078B
            retval = fdctrl_read_statusB(fdctrl);
        } else {
            retval = (uint32_t)(-1);
        }
421
        break;
422
    case 0x01:
423
424
        retval = fdctrl_read_statusB(fdctrl);
        break;
425
    case 0x02:
426
427
        retval = fdctrl_read_dor(fdctrl);
        break;
428
    case 0x03:
429
        retval = fdctrl_read_tape(fdctrl);
430
        break;
431
    case 0x04:
432
        retval = fdctrl_read_main_status(fdctrl);
433
        break;
434
    case 0x05:
435
        retval = fdctrl_read_data(fdctrl);
436
        break;
437
    case 0x07:
438
        retval = fdctrl_read_dir(fdctrl);
439
        break;
440
    default:
441
442
        retval = (uint32_t)(-1);
        break;
443
    }
444
    FLOPPY_DPRINTF("read reg%d: 0x%02x\n", reg & 7, retval);
445
446
447
448
449
450
451
452

    return retval;
}

static void fdctrl_write (void *opaque, uint32_t reg, uint32_t value)
{
    fdctrl_t *fdctrl = opaque;
453
454
    FLOPPY_DPRINTF("write reg%d: 0x%02x\n", reg & 7, value);
455
456
    switch (reg & 0x07) {
    case 0x02:
457
458
        fdctrl_write_dor(fdctrl, value);
        break;
459
    case 0x03:
460
        fdctrl_write_tape(fdctrl, value);
461
        break;
462
    case 0x04:
463
        fdctrl_write_rate(fdctrl, value);
464
        break;
465
    case 0x05:
466
        fdctrl_write_data(fdctrl, value);
467
        break;
468
    default:
469
        break;
470
    }
471
472
}
473
474
static uint32_t fdctrl_read_mem (void *opaque, target_phys_addr_t reg)
{
475
    return fdctrl_read(opaque, (uint32_t)reg);
476
477
}
478
static void fdctrl_write_mem (void *opaque,
479
480
                              target_phys_addr_t reg, uint32_t value)
{
481
    fdctrl_write(opaque, (uint32_t)reg, value);
482
483
}
bellard authored
484
static CPUReadMemoryFunc *fdctrl_mem_read[3] = {
485
486
487
    fdctrl_read_mem,
    fdctrl_read_mem,
    fdctrl_read_mem,
bellard authored
488
489
490
};

static CPUWriteMemoryFunc *fdctrl_mem_write[3] = {
491
492
493
    fdctrl_write_mem,
    fdctrl_write_mem,
    fdctrl_write_mem,
bellard authored
494
495
};
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
static void fd_save (QEMUFile *f, fdrive_t *fd)
{
    uint8_t tmp;

    tmp = fd->drflags;
    qemu_put_8s(f, &tmp);
    qemu_put_8s(f, &fd->head);
    qemu_put_8s(f, &fd->track);
    qemu_put_8s(f, &fd->sect);
    qemu_put_8s(f, &fd->dir);
    qemu_put_8s(f, &fd->rw);
}

static void fdc_save (QEMUFile *f, void *opaque)
{
    fdctrl_t *s = opaque;

    qemu_put_8s(f, &s->state);
    qemu_put_8s(f, &s->dma_en);
    qemu_put_8s(f, &s->cur_drv);
    qemu_put_8s(f, &s->bootsel);
    qemu_put_buffer(f, s->fifo, FD_SECTOR_LEN);
    qemu_put_be32s(f, &s->data_pos);
    qemu_put_be32s(f, &s->data_len);
    qemu_put_8s(f, &s->data_state);
    qemu_put_8s(f, &s->data_dir);
    qemu_put_8s(f, &s->int_status);
    qemu_put_8s(f, &s->eot);
    qemu_put_8s(f, &s->timer0);
    qemu_put_8s(f, &s->timer1);
    qemu_put_8s(f, &s->precomp_trk);
    qemu_put_8s(f, &s->config);
    qemu_put_8s(f, &s->lock);
    qemu_put_8s(f, &s->pwrd);
    fd_save(f, &s->drives[0]);
    fd_save(f, &s->drives[1]);
}

static int fd_load (QEMUFile *f, fdrive_t *fd)
{
    uint8_t tmp;

    qemu_get_8s(f, &tmp);
    fd->drflags = tmp;
    qemu_get_8s(f, &fd->head);
    qemu_get_8s(f, &fd->track);
    qemu_get_8s(f, &fd->sect);
    qemu_get_8s(f, &fd->dir);
    qemu_get_8s(f, &fd->rw);

    return 0;
}

static int fdc_load (QEMUFile *f, void *opaque, int version_id)
{
    fdctrl_t *s = opaque;
    int ret;

    if (version_id != 1)
        return -EINVAL;

    qemu_get_8s(f, &s->state);
    qemu_get_8s(f, &s->dma_en);
    qemu_get_8s(f, &s->cur_drv);
    qemu_get_8s(f, &s->bootsel);
    qemu_get_buffer(f, s->fifo, FD_SECTOR_LEN);
    qemu_get_be32s(f, &s->data_pos);
    qemu_get_be32s(f, &s->data_len);
    qemu_get_8s(f, &s->data_state);
    qemu_get_8s(f, &s->data_dir);
    qemu_get_8s(f, &s->int_status);
    qemu_get_8s(f, &s->eot);
    qemu_get_8s(f, &s->timer0);
    qemu_get_8s(f, &s->timer1);
    qemu_get_8s(f, &s->precomp_trk);
    qemu_get_8s(f, &s->config);
    qemu_get_8s(f, &s->lock);
    qemu_get_8s(f, &s->pwrd);

    ret = fd_load(f, &s->drives[0]);
    if (ret == 0)
        ret = fd_load(f, &s->drives[1]);

    return ret;
}

static void fdctrl_external_reset(void *opaque)
{
    fdctrl_t *s = opaque;

    fdctrl_reset(s, 0);
}
589
fdctrl_t *fdctrl_init (qemu_irq irq, int dma_chann, int mem_mapped,
590
                       target_phys_addr_t io_base,
591
                       BlockDriverState **fds)
592
{
593
    fdctrl_t *fdctrl;
bellard authored
594
    int io_mem;
595
596
    int i;
bellard authored
597
    FLOPPY_DPRINTF("init controller\n");
598
599
600
    fdctrl = qemu_mallocz(sizeof(fdctrl_t));
    if (!fdctrl)
        return NULL;
601
    fdctrl->result_timer = qemu_new_timer(vm_clock,
602
603
                                          fdctrl_result_timer, fdctrl);
bellard authored
604
    fdctrl->version = 0x90; /* Intel 82078 controller */
pbrook authored
605
    fdctrl->irq = irq;
606
607
    fdctrl->dma_chann = dma_chann;
    fdctrl->io_base = io_base;
608
    fdctrl->config = 0x60; /* Implicit seek, polling & FIFO enabled */
blueswir1 authored
609
    fdctrl->sun4m = 0;
610
611
612
    if (fdctrl->dma_chann != -1) {
        fdctrl->dma_en = 1;
        DMA_register_channel(dma_chann, &fdctrl_transfer_handler, fdctrl);
613
    } else {
614
        fdctrl->dma_en = 0;
615
    }
616
617
    for (i = 0; i < 2; i++) {
        fd_init(&fdctrl->drives[i], fds[i]);
618
    }
619
620
    fdctrl_reset(fdctrl, 0);
    fdctrl->state = FD_CTRL_ACTIVE;
621
    if (mem_mapped) {
622
623
        io_mem = cpu_register_io_memory(0, fdctrl_mem_read, fdctrl_mem_write,
                                        fdctrl);
bellard authored
624
        cpu_register_physical_memory(io_base, 0x08, io_mem);
625
    } else {
626
627
628
629
630
631
632
633
        register_ioport_read((uint32_t)io_base + 0x01, 5, 1, &fdctrl_read,
                             fdctrl);
        register_ioport_read((uint32_t)io_base + 0x07, 1, 1, &fdctrl_read,
                             fdctrl);
        register_ioport_write((uint32_t)io_base + 0x01, 5, 1, &fdctrl_write,
                              fdctrl);
        register_ioport_write((uint32_t)io_base + 0x07, 1, 1, &fdctrl_write,
                              fdctrl);
634
    }
635
636
    register_savevm("fdc", io_base, 1, fdc_save, fdc_load, fdctrl);
    qemu_register_reset(fdctrl_external_reset, fdctrl);
637
    for (i = 0; i < 2; i++) {
638
        fd_revalidate(&fdctrl->drives[i]);
639
    }
640
641
    return fdctrl;
642
}
643
644
645
646
647
648
649
fdctrl_t *sun4m_fdctrl_init (qemu_irq irq, target_phys_addr_t io_base,
                             BlockDriverState **fds)
{
    fdctrl_t *fdctrl;

    fdctrl = fdctrl_init(irq, 0, 1, io_base, fds);
blueswir1 authored
650
    fdctrl->sun4m = 1;
651
652
653
654

    return fdctrl;
}
655
656
/* XXX: may change if moved to bdrv */
int fdctrl_get_drive_type(fdctrl_t *fdctrl, int drive_num)
657
{
658
    return fdctrl->drives[drive_num].drive;
659
660
661
}

/* Change IRQ state */
662
static void fdctrl_reset_irq (fdctrl_t *fdctrl)
663
{
664
    FLOPPY_DPRINTF("Reset interrupt\n");
pbrook authored
665
    qemu_set_irq(fdctrl->irq, 0);
666
    fdctrl->state &= ~FD_CTRL_INTR;
667
668
}
669
static void fdctrl_raise_irq (fdctrl_t *fdctrl, uint8_t status)
670
{
bellard authored
671
    // Sparc mutation
blueswir1 authored
672
    if (fdctrl->sun4m && !fdctrl->dma_en) {
673
674
675
        fdctrl->state &= ~FD_CTRL_BUSY;
        fdctrl->int_status = status;
        return;
bellard authored
676
    }
677
    if (~(fdctrl->state & FD_CTRL_INTR)) {
pbrook authored
678
        qemu_set_irq(fdctrl->irq, 1);
679
        fdctrl->state |= FD_CTRL_INTR;
680
681
    }
    FLOPPY_DPRINTF("Set interrupt status to 0x%02x\n", status);
682
    fdctrl->int_status = status;
683
684
}
bellard authored
685
/* Reset controller */
686
static void fdctrl_reset (fdctrl_t *fdctrl, int do_irq)
687
688
689
{
    int i;
bellard authored
690
    FLOPPY_DPRINTF("reset controller\n");
691
    fdctrl_reset_irq(fdctrl);
bellard authored
692
    /* Initialise controller */
693
    fdctrl->cur_drv = 0;
694
    /* FIFO state */
695
696
697
698
    fdctrl->data_pos = 0;
    fdctrl->data_len = 0;
    fdctrl->data_state = FD_STATE_CMD;
    fdctrl->data_dir = FD_DIR_WRITE;
699
    for (i = 0; i < MAX_FD; i++)
700
701
        fd_reset(&fdctrl->drives[i]);
    fdctrl_reset_fifo(fdctrl);
702
    if (do_irq)
703
        fdctrl_raise_irq(fdctrl, 0xc0);
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
}

static inline fdrive_t *drv0 (fdctrl_t *fdctrl)
{
    return &fdctrl->drives[fdctrl->bootsel];
}

static inline fdrive_t *drv1 (fdctrl_t *fdctrl)
{
    return &fdctrl->drives[1 - fdctrl->bootsel];
}

static fdrive_t *get_cur_drv (fdctrl_t *fdctrl)
{
    return fdctrl->cur_drv == 0 ? drv0(fdctrl) : drv1(fdctrl);
719
720
721
}

/* Status B register : 0x01 (read-only) */
722
static uint32_t fdctrl_read_statusB (fdctrl_t *fdctrl)
723
724
725
726
727
728
{
    FLOPPY_DPRINTF("status register: 0x00\n");
    return 0;
}

/* Digital output register : 0x02 */
729
static uint32_t fdctrl_read_dor (fdctrl_t *fdctrl)
730
731
732
733
{
    uint32_t retval = 0;

    /* Drive motors state indicators */
734
    if (drv0(fdctrl)->drflags & FDRIVE_MOTOR_ON)
735
        retval |= 1 << 5;
736
    if (drv1(fdctrl)->drflags & FDRIVE_MOTOR_ON)
737
        retval |= 1 << 4;
738
    /* DMA enable */
739
    retval |= fdctrl->dma_en << 3;
740
    /* Reset indicator */
741
    retval |= (fdctrl->state & FD_CTRL_RESET) == 0 ? 0x04 : 0;
742
    /* Selected drive */
743
    retval |= fdctrl->cur_drv;
744
745
746
747
748
    FLOPPY_DPRINTF("digital output register: 0x%02x\n", retval);

    return retval;
}
749
static void fdctrl_write_dor (fdctrl_t *fdctrl, uint32_t value)
750
751
{
    /* Reset mode */
752
    if (fdctrl->state & FD_CTRL_RESET) {
753
        if (!(value & 0x04)) {
bellard authored
754
            FLOPPY_DPRINTF("Floppy controller in RESET state !\n");
755
756
757
758
759
760
            return;
        }
    }
    FLOPPY_DPRINTF("digital output register set to 0x%02x\n", value);
    /* Drive motors state indicators */
    if (value & 0x20)
761
        fd_start(drv1(fdctrl));
762
    else
763
        fd_stop(drv1(fdctrl));
764
    if (value & 0x10)
765
        fd_start(drv0(fdctrl));
766
    else
767
        fd_stop(drv0(fdctrl));
768
769
    /* DMA enable */
#if 0
770
771
    if (fdctrl->dma_chann != -1)
        fdctrl->dma_en = 1 - ((value >> 3) & 1);
772
773
774
#endif
    /* Reset */
    if (!(value & 0x04)) {
775
        if (!(fdctrl->state & FD_CTRL_RESET)) {
bellard authored
776
            FLOPPY_DPRINTF("controller enter RESET state\n");
777
            fdctrl->state |= FD_CTRL_RESET;
778
779
        }
    } else {
780
        if (fdctrl->state & FD_CTRL_RESET) {
bellard authored
781
            FLOPPY_DPRINTF("controller out of RESET state\n");
782
            fdctrl_reset(fdctrl, 1);
783
            fdctrl->state &= ~(FD_CTRL_RESET | FD_CTRL_SLEEP);
784
785
786
        }
    }
    /* Selected drive */
787
    fdctrl->cur_drv = value & 1;
788
789
790
}

/* Tape drive register : 0x03 */
791
static uint32_t fdctrl_read_tape (fdctrl_t *fdctrl)
792
793
794
795
{
    uint32_t retval = 0;

    /* Disk boot selection indicator */
796
    retval |= fdctrl->bootsel << 2;
797
798
799
800
801
802
    /* Tape indicators: never allowed */
    FLOPPY_DPRINTF("tape drive register: 0x%02x\n", retval);

    return retval;
}
803
static void fdctrl_write_tape (fdctrl_t *fdctrl, uint32_t value)
804
805
{
    /* Reset mode */
806
    if (fdctrl->state & FD_CTRL_RESET) {
bellard authored
807
        FLOPPY_DPRINTF("Floppy controller in RESET state !\n");
808
809
810
811
        return;
    }
    FLOPPY_DPRINTF("tape drive register set to 0x%02x\n", value);
    /* Disk boot selection indicator */
812
    fdctrl->bootsel = (value >> 2) & 1;
813
814
815
816
    /* Tape indicators: never allow */
}

/* Main status register : 0x04 (read) */
817
static uint32_t fdctrl_read_main_status (fdctrl_t *fdctrl)
818
819
820
{
    uint32_t retval = 0;
821
822
    fdctrl->state &= ~(FD_CTRL_SLEEP | FD_CTRL_RESET);
    if (!(fdctrl->state & FD_CTRL_BUSY)) {
823
824
825
        /* Data transfer allowed */
        retval |= 0x80;
        /* Data transfer direction indicator */
826
        if (fdctrl->data_dir == FD_DIR_READ)
827
828
829
830
            retval |= 0x40;
    }
    /* Should handle 0x20 for SPECIFY command */
    /* Command busy indicator */
831
832
    if (FD_STATE(fdctrl->data_state) == FD_STATE_DATA ||
        FD_STATE(fdctrl->data_state) == FD_STATE_STATUS)
833
834
835
836
837
838
839
        retval |= 0x10;
    FLOPPY_DPRINTF("main status register: 0x%02x\n", retval);

    return retval;
}

/* Data select rate register : 0x04 (write) */
840
static void fdctrl_write_rate (fdctrl_t *fdctrl, uint32_t value)
841
842
{
    /* Reset mode */
843
    if (fdctrl->state & FD_CTRL_RESET) {
844
845
846
        FLOPPY_DPRINTF("Floppy controller in RESET state !\n");
        return;
    }
847
848
849
    FLOPPY_DPRINTF("select rate register set to 0x%02x\n", value);
    /* Reset: autoclear */
    if (value & 0x80) {
850
851
852
        fdctrl->state |= FD_CTRL_RESET;
        fdctrl_reset(fdctrl, 1);
        fdctrl->state &= ~FD_CTRL_RESET;
853
854
    }
    if (value & 0x40) {
855
856
        fdctrl->state |= FD_CTRL_SLEEP;
        fdctrl_reset(fdctrl, 1);
857
858
859
860
    }
//        fdctrl.precomp = (value >> 2) & 0x07;
}
bellard authored
861
862
863
static int fdctrl_media_changed(fdrive_t *drv)
{
    int ret;
864
865
    if (!drv->bs)
bellard authored
866
867
868
869
870
871
872
873
        return 0;
    ret = bdrv_media_changed(drv->bs);
    if (ret) {
        fd_revalidate(drv);
    }
    return ret;
}
874
/* Digital input register : 0x07 (read-only) */
875
static uint32_t fdctrl_read_dir (fdctrl_t *fdctrl)
876
877
878
{
    uint32_t retval = 0;
bellard authored
879
    if (fdctrl_media_changed(drv0(fdctrl)) ||
880
        fdctrl_media_changed(drv1(fdctrl)))
881
882
        retval |= 0x80;
    if (retval != 0)
883
        FLOPPY_DPRINTF("Floppy digital input register: 0x%02x\n", retval);
884
885
886
887
888

    return retval;
}

/* FIFO state control */
889
static void fdctrl_reset_fifo (fdctrl_t *fdctrl)
890
{
891
892
893
    fdctrl->data_dir = FD_DIR_WRITE;
    fdctrl->data_pos = 0;
    FD_SET_STATE(fdctrl->data_state, FD_STATE_CMD);
894
895
896
}

/* Set FIFO status for the host to read */
897
static void fdctrl_set_fifo (fdctrl_t *fdctrl, int fifo_len, int do_irq)
898
{
899
900
901
902
    fdctrl->data_dir = FD_DIR_READ;
    fdctrl->data_len = fifo_len;
    fdctrl->data_pos = 0;
    FD_SET_STATE(fdctrl->data_state, FD_STATE_STATUS);
903
    if (do_irq)
904
        fdctrl_raise_irq(fdctrl, 0x00);
905
906
907
}

/* Set an error: unimplemented/unknown command */
908
static void fdctrl_unimplemented (fdctrl_t *fdctrl)
909
910
{
#if 0
911
912
913
    fdrive_t *cur_drv;

    cur_drv = get_cur_drv(fdctrl);
914
    fdctrl->fifo[0] = 0x60 | (cur_drv->head << 2) | fdctrl->cur_drv;
915
916
917
    fdctrl->fifo[1] = 0x00;
    fdctrl->fifo[2] = 0x00;
    fdctrl_set_fifo(fdctrl, 3, 1);
918
#else
919
920
921
    //    fdctrl_reset_fifo(fdctrl);
    fdctrl->fifo[0] = 0x80;
    fdctrl_set_fifo(fdctrl, 1, 0);
922
923
924
925
#endif
}

/* Callback for transfer end (stop or abort) */
926
static void fdctrl_stop_transfer (fdctrl_t *fdctrl, uint8_t status0,
927
                                  uint8_t status1, uint8_t status2)
928
{
929
    fdrive_t *cur_drv;
930
931
    cur_drv = get_cur_drv(fdctrl);
932
933
    FLOPPY_DPRINTF("transfer status: %02x %02x %02x (%02x)\n",
                   status0, status1, status2,
934
935
                   status0 | (cur_drv->head << 2) | fdctrl->cur_drv);
    fdctrl->fifo[0] = status0 | (cur_drv->head << 2) | fdctrl->cur_drv;
936
937
938
939
940
941
942
    fdctrl->fifo[1] = status1;
    fdctrl->fifo[2] = status2;
    fdctrl->fifo[3] = cur_drv->track;
    fdctrl->fifo[4] = cur_drv->head;
    fdctrl->fifo[5] = cur_drv->sect;
    fdctrl->fifo[6] = FD_SECTOR_SC;
    fdctrl->data_dir = FD_DIR_READ;
943
    if (fdctrl->state & FD_CTRL_BUSY) {
944
        DMA_release_DREQ(fdctrl->dma_chann);
945
946
        fdctrl->state &= ~FD_CTRL_BUSY;
    }
947
    fdctrl_set_fifo(fdctrl, 7, 1);
948
949
950
}

/* Prepare a data transfer (either DMA or FIFO) */
951
static void fdctrl_start_transfer (fdctrl_t *fdctrl, int direction)
952
{
953
    fdrive_t *cur_drv;
954
955
956
    uint8_t kh, kt, ks;
    int did_seek;
957
958
959
960
961
    fdctrl->cur_drv = fdctrl->fifo[1] & 1;
    cur_drv = get_cur_drv(fdctrl);
    kt = fdctrl->fifo[2];
    kh = fdctrl->fifo[3];
    ks = fdctrl->fifo[4];
bellard authored
962
    FLOPPY_DPRINTF("Start transfer at %d %d %02x %02x (%d)\n",
963
                   fdctrl->cur_drv, kh, kt, ks,
964
965
                   _fd_sector(kh, kt, ks, cur_drv->last_sect));
    did_seek = 0;
966
    switch (fd_seek(cur_drv, kh, kt, ks, fdctrl->config & 0x40)) {
967
968
    case 2:
        /* sect too big */
969
970
971
972
        fdctrl_stop_transfer(fdctrl, 0x40, 0x00, 0x00);
        fdctrl->fifo[3] = kt;
        fdctrl->fifo[4] = kh;
        fdctrl->fifo[5] = ks;
973
974
975
        return;
    case 3:
        /* track too big */
976
977
978
979
        fdctrl_stop_transfer(fdctrl, 0x40, 0x80, 0x00);
        fdctrl->fifo[3] = kt;
        fdctrl->fifo[4] = kh;
        fdctrl->fifo[5] = ks;
980
981
982
        return;
    case 4:
        /* No seek enabled */
983
984
985
986
        fdctrl_stop_transfer(fdctrl, 0x40, 0x00, 0x00);
        fdctrl->fifo[3] = kt;
        fdctrl->fifo[4] = kh;
        fdctrl->fifo[5] = ks;
987
988
989
990
991
992
993
994
        return;
    case 1:
        did_seek = 1;
        break;
    default:
        break;
    }
    /* Set the FIFO state */
995
996
997
998
999
1000
1001
    fdctrl->data_dir = direction;
    fdctrl->data_pos = 0;
    FD_SET_STATE(fdctrl->data_state, FD_STATE_DATA); /* FIFO ready for data */
    if (fdctrl->fifo[0] & 0x80)
        fdctrl->data_state |= FD_STATE_MULTI;
    else
        fdctrl->data_state &= ~FD_STATE_MULTI;
1002
    if (did_seek)
1003
1004
1005
1006
1007
1008
        fdctrl->data_state |= FD_STATE_SEEK;
    else
        fdctrl->data_state &= ~FD_STATE_SEEK;
    if (fdctrl->fifo[5] == 00) {
        fdctrl->data_len = fdctrl->fifo[8];
    } else {
1009
        int tmp;
1010
        fdctrl->data_len = 128 << (fdctrl->fifo[5] > 7 ? 7 : fdctrl->fifo[5]);
1011
1012
1013
        tmp = (cur_drv->last_sect - ks + 1);
        if (fdctrl->fifo[0] & 0x80)
            tmp += cur_drv->last_sect;
1014
        fdctrl->data_len *= tmp;
1015
    }
1016
    fdctrl->eot = fdctrl->fifo[6];
1017
    if (fdctrl->dma_en) {
1018
1019
        int dma_mode;
        /* DMA transfer are enabled. Check if DMA channel is well programmed */
1020
        dma_mode = DMA_get_channel_mode(fdctrl->dma_chann);
1021
        dma_mode = (dma_mode >> 2) & 3;
1022
        FLOPPY_DPRINTF("dma_mode=%d direction=%d (%d - %d)\n",
1023
                       dma_mode, direction,
1024
                       (128 << fdctrl->fifo[5]) *
1025
                       (cur_drv->last_sect - ks + 1), fdctrl->data_len);
1026
1027
1028
1029
1030
        if (((direction == FD_DIR_SCANE || direction == FD_DIR_SCANL ||
              direction == FD_DIR_SCANH) && dma_mode == 0) ||
            (direction == FD_DIR_WRITE && dma_mode == 2) ||
            (direction == FD_DIR_READ && dma_mode == 1)) {
            /* No access is allowed until DMA transfer has completed */
1031
            fdctrl->state |= FD_CTRL_BUSY;
bellard authored
1032
            /* Now, we just have to wait for the DMA controller to
1033
1034
             * recall us...
             */
1035
1036
            DMA_hold_DREQ(fdctrl->dma_chann);
            DMA_schedule(fdctrl->dma_chann);
1037
            return;
1038
        } else {
1039
            FLOPPY_ERROR("dma_mode=%d direction=%d\n", dma_mode, direction);
1040
1041
1042
1043
        }
    }
    FLOPPY_DPRINTF("start non-DMA transfer\n");
    /* IO based transfer: calculate len */
1044
    fdctrl_raise_irq(fdctrl, 0x00);
1045
1046
1047
1048
1049

    return;
}

/* Prepare a transfer of deleted data */
1050
static void fdctrl_start_transfer_del (fdctrl_t *fdctrl, int direction)
1051
1052
1053
1054
{
    /* We don't handle deleted data,
     * so we don't return *ANYTHING*
     */
1055
    fdctrl_stop_transfer(fdctrl, 0x60, 0x00, 0x00);
1056
1057
1058
}

/* handlers for DMA transfers */
bellard authored
1059
1060
static int fdctrl_transfer_handler (void *opaque, int nchan,
                                    int dma_pos, int dma_len)
1061
{
1062
1063
1064
    fdctrl_t *fdctrl;
    fdrive_t *cur_drv;
    int len, start_pos, rel_pos;
1065
1066
    uint8_t status0 = 0x00, status1 = 0x00, status2 = 0x00;
1067
1068
    fdctrl = opaque;
    if (!(fdctrl->state & FD_CTRL_BUSY)) {
1069
1070
1071
        FLOPPY_DPRINTF("Not in DMA transfer mode !\n");
        return 0;
    }
1072
1073
1074
    cur_drv = get_cur_drv(fdctrl);
    if (fdctrl->data_dir == FD_DIR_SCANE || fdctrl->data_dir == FD_DIR_SCANL ||
        fdctrl->data_dir == FD_DIR_SCANH)
1075
        status2 = 0x04;
bellard authored
1076
1077
    if (dma_len > fdctrl->data_len)
        dma_len = fdctrl->data_len;
1078
    if (cur_drv->bs == NULL) {
1079
1080
1081
1082
1083
        if (fdctrl->data_dir == FD_DIR_WRITE)
            fdctrl_stop_transfer(fdctrl, 0x60, 0x00, 0x00);
        else
            fdctrl_stop_transfer(fdctrl, 0x40, 0x00, 0x00);
        len = 0;
1084
1085
        goto transfer_error;
    }
1086
    rel_pos = fdctrl->data_pos % FD_SECTOR_LEN;
bellard authored
1087
1088
    for (start_pos = fdctrl->data_pos; fdctrl->data_pos < dma_len;) {
        len = dma_len - fdctrl->data_pos;
1089
1090
        if (len + rel_pos > FD_SECTOR_LEN)
            len = FD_SECTOR_LEN - rel_pos;
bellard authored
1091
1092
        FLOPPY_DPRINTF("copy %d bytes (%d %d %d) %d pos %d %02x "
                       "(%d-0x%08x 0x%08x)\n", len, dma_len, fdctrl->data_pos,
1093
1094
                       fdctrl->data_len, fdctrl->cur_drv, cur_drv->head,
                       cur_drv->track, cur_drv->sect, fd_sector(cur_drv),
bellard authored
1095
                       fd_sector(cur_drv) * 512);
1096
        if (fdctrl->data_dir != FD_DIR_WRITE ||
1097
            len < FD_SECTOR_LEN || rel_pos != 0) {
1098
1099
            /* READ & SCAN commands and realign to a sector for WRITE */
            if (bdrv_read(cur_drv->bs, fd_sector(cur_drv),
1100
                          fdctrl->fifo, 1) < 0) {
1101
1102
1103
                FLOPPY_DPRINTF("Floppy: error getting sector %d\n",
                               fd_sector(cur_drv));
                /* Sure, image size is too small... */
1104
                memset(fdctrl->fifo, 0, FD_SECTOR_LEN);
1105
            }
1106
        }
1107
1108
1109
        switch (fdctrl->data_dir) {
        case FD_DIR_READ:
            /* READ commands */
bellard authored
1110
1111
            DMA_write_memory (nchan, fdctrl->fifo + rel_pos,
                              fdctrl->data_pos, len);
1112
1113
            break;
        case FD_DIR_WRITE:
1114
            /* WRITE commands */
bellard authored
1115
1116
            DMA_read_memory (nchan, fdctrl->fifo + rel_pos,
                             fdctrl->data_pos, len);
1117
            if (bdrv_write(cur_drv->bs, fd_sector(cur_drv),
1118
                           fdctrl->fifo, 1) < 0) {
1119
1120
1121
                FLOPPY_ERROR("writting sector %d\n", fd_sector(cur_drv));
                fdctrl_stop_transfer(fdctrl, 0x60, 0x00, 0x00);
                goto transfer_error;
1122
            }
1123
1124
1125
            break;
        default:
            /* SCAN commands */
1126
            {
1127
                uint8_t tmpbuf[FD_SECTOR_LEN];
1128
                int ret;
bellard authored
1129
                DMA_read_memory (nchan, tmpbuf, fdctrl->data_pos, len);
1130
                ret = memcmp(tmpbuf, fdctrl->fifo + rel_pos, len);
1131
1132
1133
1134
                if (ret == 0) {
                    status2 = 0x08;
                    goto end_transfer;
                }
1135
1136
                if ((ret < 0 && fdctrl->data_dir == FD_DIR_SCANL) ||
                    (ret > 0 && fdctrl->data_dir == FD_DIR_SCANH)) {
1137
1138
1139
1140
                    status2 = 0x00;
                    goto end_transfer;
                }
            }
1141
            break;
1142
        }
1143
1144
        fdctrl->data_pos += len;
        rel_pos = fdctrl->data_pos % FD_SECTOR_LEN;
1145
        if (rel_pos == 0) {
1146
            /* Seek to next sector */
1147
1148
1149
1150
            FLOPPY_DPRINTF("seek to next sector (%d %02x %02x => %d) (%d)\n",
                           cur_drv->head, cur_drv->track, cur_drv->sect,
                           fd_sector(cur_drv),
                           fdctrl->data_pos - len);
1151
1152
1153
1154
            /* XXX: cur_drv->sect >= cur_drv->last_sect should be an
               error in fact */
            if (cur_drv->sect >= cur_drv->last_sect ||
                cur_drv->sect == fdctrl->eot) {
1155
1156
1157
1158
                cur_drv->sect = 1;
                if (FD_MULTI_TRACK(fdctrl->data_state)) {
                    if (cur_drv->head == 0 &&
                        (cur_drv->flags & FDISK_DBL_SIDES) != 0) {
1159
1160
1161
                        cur_drv->head = 1;
                    } else {
                        cur_drv->head = 0;
1162
1163
1164
                        cur_drv->track++;
                        if ((cur_drv->flags & FDISK_DBL_SIDES) == 0)
                            break;
1165
1166
1167
1168
                    }
                } else {
                    cur_drv->track++;
                    break;
1169
                }
1170
1171
1172
                FLOPPY_DPRINTF("seek to next track (%d %02x %02x => %d)\n",
                               cur_drv->head, cur_drv->track,
                               cur_drv->sect, fd_sector(cur_drv));
1173
1174
            } else {
                cur_drv->sect++;
1175
1176
1177
            }
        }
    }
1178
 end_transfer:
1179
1180
    len = fdctrl->data_pos - start_pos;
    FLOPPY_DPRINTF("end transfer %d %d %d\n",
1181
                   fdctrl->data_pos, len, fdctrl->data_len);
1182
1183
1184
    if (fdctrl->data_dir == FD_DIR_SCANE ||
        fdctrl->data_dir == FD_DIR_SCANL ||
        fdctrl->data_dir == FD_DIR_SCANH)
1185
        status2 = 0x08;
1186
    if (FD_DID_SEEK(fdctrl->data_state))
1187
        status0 |= 0x20;
1188
1189
    fdctrl->data_len -= len;
    //    if (fdctrl->data_len == 0)
1190
    fdctrl_stop_transfer(fdctrl, status0, status1, status2);
1191
 transfer_error:
1192
1193
    return len;
1194
1195
1196
}

/* Data register : 0x05 */
1197
static uint32_t fdctrl_read_data (fdctrl_t *fdctrl)
1198
{
1199
    fdrive_t *cur_drv;
1200
1201
1202
    uint32_t retval = 0;
    int pos, len;
1203
1204
1205
    cur_drv = get_cur_drv(fdctrl);
    fdctrl->state &= ~FD_CTRL_SLEEP;
    if (FD_STATE(fdctrl->data_state) == FD_STATE_CMD) {
1206
1207
1208
        FLOPPY_ERROR("can't read data in CMD state\n");
        return 0;
    }
1209
1210
    pos = fdctrl->data_pos;
    if (FD_STATE(fdctrl->data_state) == FD_STATE_DATA) {
1211
1212
        pos %= FD_SECTOR_LEN;
        if (pos == 0) {
1213
            len = fdctrl->data_len - fdctrl->data_pos;
1214
1215
            if (len > FD_SECTOR_LEN)
                len = FD_SECTOR_LEN;
1216
            bdrv_read(cur_drv->bs, fd_sector(cur_drv), fdctrl->fifo, 1);
1217
1218
        }
    }
1219
1220
1221
    retval = fdctrl->fifo[pos];
    if (++fdctrl->data_pos == fdctrl->data_len) {
        fdctrl->data_pos = 0;
1222
        /* Switch from transfer mode to status mode
1223
1224
         * then from status mode to command mode
         */
1225
        if (FD_STATE(fdctrl->data_state) == FD_STATE_DATA) {
1226
            fdctrl_stop_transfer(fdctrl, 0x20, 0x00, 0x00);
1227
        } else {
1228
            fdctrl_reset_fifo(fdctrl);
1229
1230
            fdctrl_reset_irq(fdctrl);
        }
1231
1232
1233
1234
1235
1236
    }
    FLOPPY_DPRINTF("data register: 0x%02x\n", retval);

    return retval;
}
1237
static void fdctrl_format_sector (fdctrl_t *fdctrl)
1238
{
1239
1240
1241
    fdrive_t *cur_drv;
    uint8_t kh, kt, ks;
    int did_seek;
1242
1243
1244
1245
1246
1247
1248
1249
1250
1251
1252
1253
1254
1255
1256
1257
1258
1259
1260
1261
1262
1263
1264
1265
1266
1267
1268
1269
1270
1271
1272
1273
1274
1275
1276
1277
1278
1279
1280
1281
1282
1283
    fdctrl->cur_drv = fdctrl->fifo[1] & 1;
    cur_drv = get_cur_drv(fdctrl);
    kt = fdctrl->fifo[6];
    kh = fdctrl->fifo[7];
    ks = fdctrl->fifo[8];
    FLOPPY_DPRINTF("format sector at %d %d %02x %02x (%d)\n",
                   fdctrl->cur_drv, kh, kt, ks,
                   _fd_sector(kh, kt, ks, cur_drv->last_sect));
    did_seek = 0;
    switch (fd_seek(cur_drv, kh, kt, ks, fdctrl->config & 0x40)) {
    case 2:
        /* sect too big */
        fdctrl_stop_transfer(fdctrl, 0x40, 0x00, 0x00);
        fdctrl->fifo[3] = kt;
        fdctrl->fifo[4] = kh;
        fdctrl->fifo[5] = ks;
        return;
    case 3:
        /* track too big */
        fdctrl_stop_transfer(fdctrl, 0x40, 0x80, 0x00);
        fdctrl->fifo[3] = kt;
        fdctrl->fifo[4] = kh;
        fdctrl->fifo[5] = ks;
        return;
    case 4:
        /* No seek enabled */
        fdctrl_stop_transfer(fdctrl, 0x40, 0x00, 0x00);
        fdctrl->fifo[3] = kt;
        fdctrl->fifo[4] = kh;
        fdctrl->fifo[5] = ks;
        return;
    case 1:
        did_seek = 1;
        fdctrl->data_state |= FD_STATE_SEEK;
        break;
    default:
        break;
    }
    memset(fdctrl->fifo, 0, FD_SECTOR_LEN);
    if (cur_drv->bs == NULL ||
        bdrv_write(cur_drv->bs, fd_sector(cur_drv), fdctrl->fifo, 1) < 0) {
ths authored
1284
        FLOPPY_ERROR("formatting sector %d\n", fd_sector(cur_drv));
1285
1286
        fdctrl_stop_transfer(fdctrl, 0x60, 0x00, 0x00);
    } else {
1287
1288
1289
1290
1291
1292
1293
1294
1295
1296
1297
1298
        if (cur_drv->sect == cur_drv->last_sect) {
            fdctrl->data_state &= ~FD_STATE_FORMAT;
            /* Last sector done */
            if (FD_DID_SEEK(fdctrl->data_state))
                fdctrl_stop_transfer(fdctrl, 0x20, 0x00, 0x00);
            else
                fdctrl_stop_transfer(fdctrl, 0x00, 0x00, 0x00);
        } else {
            /* More to do */
            fdctrl->data_pos = 0;
            fdctrl->data_len = 4;
        }
1299
1300
1301
1302
1303
1304
1305
1306
    }
}

static void fdctrl_write_data (fdctrl_t *fdctrl, uint32_t value)
{
    fdrive_t *cur_drv;

    cur_drv = get_cur_drv(fdctrl);
1307
    /* Reset mode */
1308
    if (fdctrl->state & FD_CTRL_RESET) {
bellard authored
1309
        FLOPPY_DPRINTF("Floppy controller in RESET state !\n");
1310
1311
        return;
    }
1312
1313
    fdctrl->state &= ~FD_CTRL_SLEEP;
    if (FD_STATE(fdctrl->data_state) == FD_STATE_STATUS) {
1314
1315
1316
1317
        FLOPPY_ERROR("can't write data in status mode\n");
        return;
    }
    /* Is it write command time ? */
1318
    if (FD_STATE(fdctrl->data_state) == FD_STATE_DATA) {
1319
        /* FIFO data write */
1320
1321
1322
        fdctrl->fifo[fdctrl->data_pos++] = value;
        if (fdctrl->data_pos % FD_SECTOR_LEN == (FD_SECTOR_LEN - 1) ||
            fdctrl->data_pos == fdctrl->data_len) {
1323
            bdrv_write(cur_drv->bs, fd_sector(cur_drv), fdctrl->fifo, 1);
1324
        }
1325
        /* Switch from transfer mode to status mode
1326
1327
         * then from status mode to command mode
         */
1328
1329
        if (FD_STATE(fdctrl->data_state) == FD_STATE_DATA)
            fdctrl_stop_transfer(fdctrl, 0x20, 0x00, 0x00);
1330
1331
        return;
    }
1332
    if (fdctrl->data_pos == 0) {
1333
1334
1335
1336
1337
1338
        /* Command */
        switch (value & 0x5F) {
        case 0x46:
            /* READ variants */
            FLOPPY_DPRINTF("READ command\n");
            /* 8 parameters cmd */
1339
            fdctrl->data_len = 9;
1340
1341
1342
1343
1344
            goto enqueue;
        case 0x4C:
            /* READ_DELETED variants */
            FLOPPY_DPRINTF("READ_DELETED command\n");
            /* 8 parameters cmd */
1345
            fdctrl->data_len = 9;
1346
1347
1348
1349
1350
            goto enqueue;
        case 0x50:
            /* SCAN_EQUAL variants */
            FLOPPY_DPRINTF("SCAN_EQUAL command\n");
            /* 8 parameters cmd */
1351
            fdctrl->data_len = 9;
1352
1353
1354
1355
1356
            goto enqueue;
        case 0x56:
            /* VERIFY variants */
            FLOPPY_DPRINTF("VERIFY command\n");
            /* 8 parameters cmd */
1357
            fdctrl->data_len = 9;
1358
1359
1360
1361
1362
            goto enqueue;
        case 0x59:
            /* SCAN_LOW_OR_EQUAL variants */
            FLOPPY_DPRINTF("SCAN_LOW_OR_EQUAL command\n");
            /* 8 parameters cmd */
1363
            fdctrl->data_len = 9;
1364
1365
1366
1367
1368
            goto enqueue;
        case 0x5D:
            /* SCAN_HIGH_OR_EQUAL variants */
            FLOPPY_DPRINTF("SCAN_HIGH_OR_EQUAL command\n");
            /* 8 parameters cmd */
1369
            fdctrl->data_len = 9;
1370
1371
1372
1373
1374
1375
1376
1377
1378
            goto enqueue;
        default:
            break;
        }
        switch (value & 0x7F) {
        case 0x45:
            /* WRITE variants */
            FLOPPY_DPRINTF("WRITE command\n");
            /* 8 parameters cmd */
1379
            fdctrl->data_len = 9;
1380
1381
1382
1383
1384
            goto enqueue;
        case 0x49:
            /* WRITE_DELETED variants */
            FLOPPY_DPRINTF("WRITE_DELETED command\n");
            /* 8 parameters cmd */
1385
            fdctrl->data_len = 9;
1386
1387
1388
1389
1390
1391
1392
1393
1394
            goto enqueue;
        default:
            break;
        }
        switch (value) {
        case 0x03:
            /* SPECIFY */
            FLOPPY_DPRINTF("SPECIFY command\n");
            /* 1 parameter cmd */
1395
            fdctrl->data_len = 3;
1396
1397
1398
1399
1400
            goto enqueue;
        case 0x04:
            /* SENSE_DRIVE_STATUS */
            FLOPPY_DPRINTF("SENSE_DRIVE_STATUS command\n");
            /* 1 parameter cmd */
1401
            fdctrl->data_len = 2;
1402
1403
1404
1405
1406
            goto enqueue;
        case 0x07:
            /* RECALIBRATE */
            FLOPPY_DPRINTF("RECALIBRATE command\n");
            /* 1 parameter cmd */
1407
            fdctrl->data_len = 2;
1408
1409
1410
1411
            goto enqueue;
        case 0x08:
            /* SENSE_INTERRUPT_STATUS */
            FLOPPY_DPRINTF("SENSE_INTERRUPT_STATUS command (%02x)\n",
1412
                           fdctrl->int_status);
1413
            /* No parameters cmd: returns status if no interrupt */
bellard authored
1414
#if 0
1415
1416
            fdctrl->fifo[0] =
                fdctrl->int_status | (cur_drv->head << 2) | fdctrl->cur_drv;
bellard authored
1417
1418
1419
1420
1421
1422
1423
#else
            /* XXX: int_status handling is broken for read/write
               commands, so we do this hack. It should be suppressed
               ASAP */
            fdctrl->fifo[0] =
                0x20 | (cur_drv->head << 2) | fdctrl->cur_drv;
#endif
1424
1425
            fdctrl->fifo[1] = cur_drv->track;
            fdctrl_set_fifo(fdctrl, 2, 0);
1426
1427
            fdctrl_reset_irq(fdctrl);
            fdctrl->int_status = 0xC0;
1428
1429
1430
1431
1432
            return;
        case 0x0E:
            /* DUMPREG */
            FLOPPY_DPRINTF("DUMPREG command\n");
            /* Drives position */
1433
1434
1435
1436
            fdctrl->fifo[0] = drv0(fdctrl)->track;
            fdctrl->fifo[1] = drv1(fdctrl)->track;
            fdctrl->fifo[2] = 0;
            fdctrl->fifo[3] = 0;
1437
            /* timers */
1438
1439
1440
1441
            fdctrl->fifo[4] = fdctrl->timer0;
            fdctrl->fifo[5] = (fdctrl->timer1 << 1) | fdctrl->dma_en;
            fdctrl->fifo[6] = cur_drv->last_sect;
            fdctrl->fifo[7] = (fdctrl->lock << 7) |
1442
                (cur_drv->perpendicular << 2);
1443
1444
1445
            fdctrl->fifo[8] = fdctrl->config;
            fdctrl->fifo[9] = fdctrl->precomp_trk;
            fdctrl_set_fifo(fdctrl, 10, 0);
1446
1447
1448
1449
1450
            return;
        case 0x0F:
            /* SEEK */
            FLOPPY_DPRINTF("SEEK command\n");
            /* 2 parameters cmd */
1451
            fdctrl->data_len = 3;
1452
1453
1454
1455
1456
            goto enqueue;
        case 0x10:
            /* VERSION */
            FLOPPY_DPRINTF("VERSION command\n");
            /* No parameters cmd */
bellard authored
1457
            /* Controller's version */
1458
1459
            fdctrl->fifo[0] = fdctrl->version;
            fdctrl_set_fifo(fdctrl, 1, 1);
1460
1461
1462
1463
1464
            return;
        case 0x12:
            /* PERPENDICULAR_MODE */
            FLOPPY_DPRINTF("PERPENDICULAR_MODE command\n");
            /* 1 parameter cmd */
1465
            fdctrl->data_len = 2;
1466
1467
1468
1469
1470
            goto enqueue;
        case 0x13:
            /* CONFIGURE */
            FLOPPY_DPRINTF("CONFIGURE command\n");
            /* 3 parameters cmd */
1471
            fdctrl->data_len = 4;
1472
1473
1474
1475
1476
            goto enqueue;
        case 0x14:
            /* UNLOCK */
            FLOPPY_DPRINTF("UNLOCK command\n");
            /* No parameters cmd */
1477
1478
1479
            fdctrl->lock = 0;
            fdctrl->fifo[0] = 0;
            fdctrl_set_fifo(fdctrl, 1, 0);
1480
1481
1482
1483
1484
            return;
        case 0x17:
            /* POWERDOWN_MODE */
            FLOPPY_DPRINTF("POWERDOWN_MODE command\n");
            /* 2 parameters cmd */
1485
            fdctrl->data_len = 3;
1486
1487
1488
1489
1490
            goto enqueue;
        case 0x18:
            /* PART_ID */
            FLOPPY_DPRINTF("PART_ID command\n");
            /* No parameters cmd */
1491
1492
            fdctrl->fifo[0] = 0x41; /* Stepping 1 */
            fdctrl_set_fifo(fdctrl, 1, 0);
1493
1494
1495
1496
1497
            return;
        case 0x2C:
            /* SAVE */
            FLOPPY_DPRINTF("SAVE command\n");
            /* No parameters cmd */
1498
1499
            fdctrl->fifo[0] = 0;
            fdctrl->fifo[1] = 0;
1500
            /* Drives position */
1501
1502
1503
1504
            fdctrl->fifo[2] = drv0(fdctrl)->track;
            fdctrl->fifo[3] = drv1(fdctrl)->track;
            fdctrl->fifo[4] = 0;
            fdctrl->fifo[5] = 0;
1505
            /* timers */
1506
1507
1508
1509
            fdctrl->fifo[6] = fdctrl->timer0;
            fdctrl->fifo[7] = fdctrl->timer1;
            fdctrl->fifo[8] = cur_drv->last_sect;
            fdctrl->fifo[9] = (fdctrl->lock << 7) |
1510
                (cur_drv->perpendicular << 2);
1511
1512
1513
1514
1515
1516
            fdctrl->fifo[10] = fdctrl->config;
            fdctrl->fifo[11] = fdctrl->precomp_trk;
            fdctrl->fifo[12] = fdctrl->pwrd;
            fdctrl->fifo[13] = 0;
            fdctrl->fifo[14] = 0;
            fdctrl_set_fifo(fdctrl, 15, 1);
1517
1518
1519
1520
1521
            return;
        case 0x33:
            /* OPTION */
            FLOPPY_DPRINTF("OPTION command\n");
            /* 1 parameter cmd */
1522
            fdctrl->data_len = 2;
1523
1524
1525
1526
1527
            goto enqueue;
        case 0x42:
            /* READ_TRACK */
            FLOPPY_DPRINTF("READ_TRACK command\n");
            /* 8 parameters cmd */
1528
            fdctrl->data_len = 9;
1529
1530
1531
1532
1533
            goto enqueue;
        case 0x4A:
            /* READ_ID */
            FLOPPY_DPRINTF("READ_ID command\n");
            /* 1 parameter cmd */
1534
            fdctrl->data_len = 2;
1535
1536
1537
1538
1539
            goto enqueue;
        case 0x4C:
            /* RESTORE */
            FLOPPY_DPRINTF("RESTORE command\n");
            /* 17 parameters cmd */
1540
            fdctrl->data_len = 18;
1541
1542
1543
1544
1545
            goto enqueue;
        case 0x4D:
            /* FORMAT_TRACK */
            FLOPPY_DPRINTF("FORMAT_TRACK command\n");
            /* 5 parameters cmd */
1546
            fdctrl->data_len = 6;
1547
1548
1549
1550
1551
            goto enqueue;
        case 0x8E:
            /* DRIVE_SPECIFICATION_COMMAND */
            FLOPPY_DPRINTF("DRIVE_SPECIFICATION_COMMAND command\n");
            /* 5 parameters cmd */
1552
            fdctrl->data_len = 6;
1553
1554
1555
1556
1557
            goto enqueue;
        case 0x8F:
            /* RELATIVE_SEEK_OUT */
            FLOPPY_DPRINTF("RELATIVE_SEEK_OUT command\n");
            /* 2 parameters cmd */
1558
            fdctrl->data_len = 3;
1559
1560
1561
1562
1563
            goto enqueue;
        case 0x94:
            /* LOCK */
            FLOPPY_DPRINTF("LOCK command\n");
            /* No parameters cmd */
1564
1565
1566
            fdctrl->lock = 1;
            fdctrl->fifo[0] = 0x10;
            fdctrl_set_fifo(fdctrl, 1, 1);
1567
1568
1569
1570
1571
            return;
        case 0xCD:
            /* FORMAT_AND_WRITE */
            FLOPPY_DPRINTF("FORMAT_AND_WRITE command\n");
            /* 10 parameters cmd */
1572
            fdctrl->data_len = 11;
1573
1574
1575
1576
1577
            goto enqueue;
        case 0xCF:
            /* RELATIVE_SEEK_IN */
            FLOPPY_DPRINTF("RELATIVE_SEEK_IN command\n");
            /* 2 parameters cmd */
1578
            fdctrl->data_len = 3;
1579
1580
1581
1582
            goto enqueue;
        default:
            /* Unknown command */
            FLOPPY_ERROR("unknown command: 0x%02x\n", value);
1583
            fdctrl_unimplemented(fdctrl);
1584
1585
1586
            return;
        }
    }
1587
 enqueue:
1588
1589
1590
    FLOPPY_DPRINTF("%s: %02x\n", __func__, value);
    fdctrl->fifo[fdctrl->data_pos] = value;
    if (++fdctrl->data_pos == fdctrl->data_len) {
1591
1592
1593
        /* We now have all parameters
         * and will be able to treat the command
         */
1594
1595
        if (fdctrl->data_state & FD_STATE_FORMAT) {
            fdctrl_format_sector(fdctrl);
1596
1597
            return;
        }
1598
1599
1600
1601
1602
1603
1604
1605
        switch (fdctrl->fifo[0] & 0x1F) {
        case 0x06:
            {
                /* READ variants */
                FLOPPY_DPRINTF("treat READ command\n");
                fdctrl_start_transfer(fdctrl, FD_DIR_READ);
                return;
            }
1606
1607
1608
1609
        case 0x0C:
            /* READ_DELETED variants */
//            FLOPPY_DPRINTF("treat READ_DELETED command\n");
            FLOPPY_ERROR("treat READ_DELETED command\n");
1610
            fdctrl_start_transfer_del(fdctrl, FD_DIR_READ);
1611
1612
1613
1614
1615
            return;
        case 0x16:
            /* VERIFY variants */
//            FLOPPY_DPRINTF("treat VERIFY command\n");
            FLOPPY_ERROR("treat VERIFY command\n");
1616
            fdctrl_stop_transfer(fdctrl, 0x20, 0x00, 0x00);
1617
1618
1619
1620
1621
            return;
        case 0x10:
            /* SCAN_EQUAL variants */
//            FLOPPY_DPRINTF("treat SCAN_EQUAL command\n");
            FLOPPY_ERROR("treat SCAN_EQUAL command\n");
1622
            fdctrl_start_transfer(fdctrl, FD_DIR_SCANE);
1623
1624
1625
1626
1627
            return;
        case 0x19:
            /* SCAN_LOW_OR_EQUAL variants */
//            FLOPPY_DPRINTF("treat SCAN_LOW_OR_EQUAL command\n");
            FLOPPY_ERROR("treat SCAN_LOW_OR_EQUAL command\n");
1628
            fdctrl_start_transfer(fdctrl, FD_DIR_SCANL);
1629
1630
1631
1632
1633
            return;
        case 0x1D:
            /* SCAN_HIGH_OR_EQUAL variants */
//            FLOPPY_DPRINTF("treat SCAN_HIGH_OR_EQUAL command\n");
            FLOPPY_ERROR("treat SCAN_HIGH_OR_EQUAL command\n");
1634
            fdctrl_start_transfer(fdctrl, FD_DIR_SCANH);
1635
1636
1637
1638
            return;
        default:
            break;
        }
1639
        switch (fdctrl->fifo[0] & 0x3F) {
1640
1641
        case 0x05:
            /* WRITE variants */
1642
1643
            FLOPPY_DPRINTF("treat WRITE command (%02x)\n", fdctrl->fifo[0]);
            fdctrl_start_transfer(fdctrl, FD_DIR_WRITE);
1644
1645
1646
1647
1648
            return;
        case 0x09:
            /* WRITE_DELETED variants */
//            FLOPPY_DPRINTF("treat WRITE_DELETED command\n");
            FLOPPY_ERROR("treat WRITE_DELETED command\n");
1649
            fdctrl_start_transfer_del(fdctrl, FD_DIR_WRITE);
1650
1651
1652
1653
            return;
        default:
            break;
        }
1654
        switch (fdctrl->fifo[0]) {
1655
1656
1657
        case 0x03:
            /* SPECIFY */
            FLOPPY_DPRINTF("treat SPECIFY command\n");
1658
            fdctrl->timer0 = (fdctrl->fifo[1] >> 4) & 0xF;
1659
            fdctrl->timer1 = fdctrl->fifo[2] >> 1;
1660
            fdctrl->dma_en = 1 - (fdctrl->fifo[2] & 1) ;
1661
            /* No result back */
1662
            fdctrl_reset_fifo(fdctrl);
1663
1664
1665
1666
            break;
        case 0x04:
            /* SENSE_DRIVE_STATUS */
            FLOPPY_DPRINTF("treat SENSE_DRIVE_STATUS command\n");
1667
            fdctrl->cur_drv = fdctrl->fifo[1] & 1;
1668
            cur_drv = get_cur_drv(fdctrl);
1669
            cur_drv->head = (fdctrl->fifo[1] >> 2) & 1;
1670
            /* 1 Byte status back */
1671
            fdctrl->fifo[0] = (cur_drv->ro << 6) |
1672
                (cur_drv->track == 0 ? 0x10 : 0x00) |
1673
1674
1675
                (cur_drv->head << 2) |
                fdctrl->cur_drv |
                0x28;
1676
            fdctrl_set_fifo(fdctrl, 1, 0);
1677
1678
1679
1680
            break;
        case 0x07:
            /* RECALIBRATE */
            FLOPPY_DPRINTF("treat RECALIBRATE command\n");
1681
            fdctrl->cur_drv = fdctrl->fifo[1] & 1;
1682
            cur_drv = get_cur_drv(fdctrl);
1683
            fd_recalibrate(cur_drv);
1684
            fdctrl_reset_fifo(fdctrl);
1685
            /* Raise Interrupt */
1686
            fdctrl_raise_irq(fdctrl, 0x20);
1687
1688
1689
1690
            break;
        case 0x0F:
            /* SEEK */
            FLOPPY_DPRINTF("treat SEEK command\n");
1691
            fdctrl->cur_drv = fdctrl->fifo[1] & 1;
1692
1693
            cur_drv = get_cur_drv(fdctrl);
            fd_start(cur_drv);
1694
            if (fdctrl->fifo[2] <= cur_drv->track)
1695
1696
1697
                cur_drv->dir = 1;
            else
                cur_drv->dir = 0;
1698
            fdctrl_reset_fifo(fdctrl);
1699
1700
            if (fdctrl->fifo[2] > cur_drv->max_track) {
                fdctrl_raise_irq(fdctrl, 0x60);
1701
            } else {
1702
                cur_drv->track = fdctrl->fifo[2];
1703
                /* Raise Interrupt */
1704
                fdctrl_raise_irq(fdctrl, 0x20);
1705
1706
1707
1708
1709
            }
            break;
        case 0x12:
            /* PERPENDICULAR_MODE */
            FLOPPY_DPRINTF("treat PERPENDICULAR_MODE command\n");
1710
1711
            if (fdctrl->fifo[1] & 0x80)
                cur_drv->perpendicular = fdctrl->fifo[1] & 0x7;
1712
            /* No result back */
1713
            fdctrl_reset_fifo(fdctrl);
1714
1715
1716
1717
            break;
        case 0x13:
            /* CONFIGURE */
            FLOPPY_DPRINTF("treat CONFIGURE command\n");
1718
1719
            fdctrl->config = fdctrl->fifo[2];
            fdctrl->precomp_trk =  fdctrl->fifo[3];
1720
            /* No result back */
1721
            fdctrl_reset_fifo(fdctrl);
1722
1723
1724
1725
            break;
        case 0x17:
            /* POWERDOWN_MODE */
            FLOPPY_DPRINTF("treat POWERDOWN_MODE command\n");
1726
1727
1728
            fdctrl->pwrd = fdctrl->fifo[1];
            fdctrl->fifo[0] = fdctrl->fifo[1];
            fdctrl_set_fifo(fdctrl, 1, 1);
1729
1730
1731
1732
1733
            break;
        case 0x33:
            /* OPTION */
            FLOPPY_DPRINTF("treat OPTION command\n");
            /* No result back */
1734
            fdctrl_reset_fifo(fdctrl);
1735
1736
1737
1738
1739
            break;
        case 0x42:
            /* READ_TRACK */
//            FLOPPY_DPRINTF("treat READ_TRACK command\n");
            FLOPPY_ERROR("treat READ_TRACK command\n");
1740
            fdctrl_start_transfer(fdctrl, FD_DIR_READ);
1741
1742
            break;
        case 0x4A:
1743
            /* READ_ID */
1744
            FLOPPY_DPRINTF("treat READ_ID command\n");
1745
            /* XXX: should set main status register to busy */
1746
            cur_drv->head = (fdctrl->fifo[1] >> 2) & 1;
1747
            qemu_mod_timer(fdctrl->result_timer,
1748
                           qemu_get_clock(vm_clock) + (ticks_per_sec / 50));
1749
1750
1751
1752
1753
            break;
        case 0x4C:
            /* RESTORE */
            FLOPPY_DPRINTF("treat RESTORE command\n");
            /* Drives position */
1754
1755
            drv0(fdctrl)->track = fdctrl->fifo[3];
            drv1(fdctrl)->track = fdctrl->fifo[4];
1756
            /* timers */
1757
1758
1759
1760
1761
1762
1763
1764
1765
            fdctrl->timer0 = fdctrl->fifo[7];
            fdctrl->timer1 = fdctrl->fifo[8];
            cur_drv->last_sect = fdctrl->fifo[9];
            fdctrl->lock = fdctrl->fifo[10] >> 7;
            cur_drv->perpendicular = (fdctrl->fifo[10] >> 2) & 0xF;
            fdctrl->config = fdctrl->fifo[11];
            fdctrl->precomp_trk = fdctrl->fifo[12];
            fdctrl->pwrd = fdctrl->fifo[13];
            fdctrl_reset_fifo(fdctrl);
1766
1767
1768
            break;
        case 0x4D:
            /* FORMAT_TRACK */
1769
1770
1771
1772
1773
1774
1775
1776
1777
1778
1779
            FLOPPY_DPRINTF("treat FORMAT_TRACK command\n");
            fdctrl->cur_drv = fdctrl->fifo[1] & 1;
            cur_drv = get_cur_drv(fdctrl);
            fdctrl->data_state |= FD_STATE_FORMAT;
            if (fdctrl->fifo[0] & 0x80)
                fdctrl->data_state |= FD_STATE_MULTI;
            else
                fdctrl->data_state &= ~FD_STATE_MULTI;
            fdctrl->data_state &= ~FD_STATE_SEEK;
            cur_drv->bps =
                fdctrl->fifo[2] > 7 ? 16384 : 128 << fdctrl->fifo[2];
1780
#if 0
1781
1782
1783
            cur_drv->last_sect =
                cur_drv->flags & FDISK_DBL_SIDES ? fdctrl->fifo[3] :
                fdctrl->fifo[3] / 2;
1784
#else
1785
            cur_drv->last_sect = fdctrl->fifo[3];
1786
#endif
1787
1788
1789
1790
1791
1792
            /* TODO: implement format using DMA expected by the Bochs BIOS
             * and Linux fdformat (read 3 bytes per sector via DMA and fill
             * the sector with the specified fill byte
             */
            fdctrl->data_state &= ~FD_STATE_FORMAT;
            fdctrl_stop_transfer(fdctrl, 0x00, 0x00, 0x00);
1793
1794
1795
1796
            break;
        case 0x8E:
            /* DRIVE_SPECIFICATION_COMMAND */
            FLOPPY_DPRINTF("treat DRIVE_SPECIFICATION_COMMAND command\n");
1797
            if (fdctrl->fifo[fdctrl->data_pos - 1] & 0x80) {
1798
                /* Command parameters done */
1799
1800
1801
1802
1803
                if (fdctrl->fifo[fdctrl->data_pos - 1] & 0x40) {
                    fdctrl->fifo[0] = fdctrl->fifo[1];
                    fdctrl->fifo[2] = 0;
                    fdctrl->fifo[3] = 0;
                    fdctrl_set_fifo(fdctrl, 4, 1);
1804
                } else {
1805
                    fdctrl_reset_fifo(fdctrl);
1806
                }
1807
            } else if (fdctrl->data_len > 7) {
1808
                /* ERROR */
1809
1810
1811
                fdctrl->fifo[0] = 0x80 |
                    (cur_drv->head << 2) | fdctrl->cur_drv;
                fdctrl_set_fifo(fdctrl, 1, 1);
1812
1813
1814
1815
1816
            }
            break;
        case 0x8F:
            /* RELATIVE_SEEK_OUT */
            FLOPPY_DPRINTF("treat RELATIVE_SEEK_OUT command\n");
1817
            fdctrl->cur_drv = fdctrl->fifo[1] & 1;
1818
1819
1820
            cur_drv = get_cur_drv(fdctrl);
            fd_start(cur_drv);
            cur_drv->dir = 0;
1821
            if (fdctrl->fifo[2] + cur_drv->track >= cur_drv->max_track) {
1822
                cur_drv->track = cur_drv->max_track - 1;
1823
1824
            } else {
                cur_drv->track += fdctrl->fifo[2];
1825
            }
1826
1827
            fdctrl_reset_fifo(fdctrl);
            fdctrl_raise_irq(fdctrl, 0x20);
1828
1829
1830
1831
1832
            break;
        case 0xCD:
            /* FORMAT_AND_WRITE */
//                FLOPPY_DPRINTF("treat FORMAT_AND_WRITE command\n");
            FLOPPY_ERROR("treat FORMAT_AND_WRITE command\n");
1833
            fdctrl_unimplemented(fdctrl);
1834
1835
            break;
        case 0xCF:
1836
            /* RELATIVE_SEEK_IN */
1837
            FLOPPY_DPRINTF("treat RELATIVE_SEEK_IN command\n");
1838
            fdctrl->cur_drv = fdctrl->fifo[1] & 1;
1839
1840
1841
            cur_drv = get_cur_drv(fdctrl);
            fd_start(cur_drv);
            cur_drv->dir = 1;
1842
            if (fdctrl->fifo[2] > cur_drv->track) {
1843
                cur_drv->track = 0;
1844
1845
            } else {
                cur_drv->track -= fdctrl->fifo[2];
1846
            }
1847
1848
1849
            fdctrl_reset_fifo(fdctrl);
            /* Raise Interrupt */
            fdctrl_raise_irq(fdctrl, 0x20);
1850
1851
1852
1853
            break;
        }
    }
}
1854
1855
1856
1857

static void fdctrl_result_timer(void *opaque)
{
    fdctrl_t *fdctrl = opaque;
1858
    fdrive_t *cur_drv = get_cur_drv(fdctrl);
1859
1860
1861
1862
1863
1864
1865
1866
    /* Pretend we are spinning.
     * This is needed for Coherent, which uses READ ID to check for
     * sector interleaving.
     */
    if (cur_drv->last_sect != 0) {
        cur_drv->sect = (cur_drv->sect % cur_drv->last_sect) + 1;
    }
1867
1868
    fdctrl_stop_transfer(fdctrl, 0x00, 0x00, 0x00);
}