Blame view

hw/fdc.c 55.9 KB
1
/*
2
 * QEMU Floppy disk emulator (Intel 82078)
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
 * 
 * Copyright (c) 2003 Jocelyn Mayer
 * 
 * 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.
 */
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
#include "vl.h"

/********************************************************/
/* 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            */
56
57
    FDRIVE_DISK_USER  = 0x04, /* User defined geometry  */
    FDRIVE_DISK_NONE  = 0x05, /* No disk                */
58
59
60
61
62
63
64
65
66
} 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;
67
68
69
70
71
72
73
74
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;
75
76
77
78
typedef struct fdrive_t {
    BlockDriverState *bs;
    /* Drive status */
    fdrive_type_t drive;
79
    fdrive_flags_t drflags;
80
81
82
83
84
85
86
87
88
    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 */
89
    fdisk_flags_t flags;
90
91
    uint8_t last_sect;        /* Nb sector per track    */
    uint8_t max_track;        /* Nb of tracks           */
92
    uint16_t bps;             /* Bytes per sector       */
93
94
95
    uint8_t ro;               /* Is read-only           */
} fdrive_t;
96
static void fd_init (fdrive_t *drv, BlockDriverState *bs)
97
98
{
    /* Drive */
99
    drv->bs = bs;
bellard authored
100
    drv->drive = FDRIVE_DRV_NONE;
101
    drv->drflags = 0;
102
103
    drv->perpendicular = 0;
    /* Disk */
104
    drv->last_sect = 0;
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
    drv->max_track = 0;
}

static int _fd_sector (uint8_t head, uint8_t track,
                        uint8_t sect, uint8_t last_sect)
{
    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;
124
125
126
127
    int ret;

    if (track > drv->max_track ||
	(head != 0 && (drv->flags & FDISK_DBL_SIDES) == 0)) {
128
129
130
131
        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);
132
133
134
        return 2;
    }
    if (sect > drv->last_sect) {
135
136
137
138
        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);
139
140
141
        return 3;
    }
    sector = _fd_sector(head, track, sect, drv->last_sect);
142
    ret = 0;
143
144
145
146
147
148
149
150
151
    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;
152
153
	if (drv->track != track)
	    ret = 1;
154
155
156
157
        drv->track = track;
        drv->sect = sect;
    }
158
    return ret;
159
160
161
162
163
164
165
166
167
168
169
170
171
}

/* 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;
}
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
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
224
225
226
227
228
/* 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;
    const unsigned char *str;
} fd_format_t;

static fd_format_t fd_formats[] = {
    /* 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", },
    /* 320 kB 5"1/4 floppy disks */ 
    { 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, },
};
229
/* Revalidate a disk drive after a disk change */
230
static void fd_revalidate (fdrive_t *drv)
231
{
232
233
234
    fd_format_t *parse;
    int64_t nb_sectors, size;
    int i, first_match, match;
235
    int nb_heads, max_track, last_sect, ro;
236
237

    FLOPPY_DPRINTF("revalidate\n");
238
    if (drv->bs != NULL && bdrv_is_inserted(drv->bs)) {
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
243
	    FLOPPY_DPRINTF("User defined disk (%d %d %d)",
                           nb_heads - 1, max_track, last_sect);
244
245
246
247
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
	} 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;
275
276
	    FLOPPY_DPRINTF("%s floppy disk (%d h %d t %d s) %s\n", parse->str,
                           nb_heads, max_track, last_sect, ro ? "ro" : "rw");
277
	}
278
279
280
281
282
283
284
285
	    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;
286
    } else {
287
	FLOPPY_DPRINTF("No disk in drive\n");
288
289
290
        drv->last_sect = 0;
	drv->max_track = 0;
	drv->flags &= ~FDISK_DBL_SIDES;
291
    }
292
293
}
294
295
296
/* Motor control */
static void fd_start (fdrive_t *drv)
{
297
    drv->drflags |= FDRIVE_MOTOR_ON;
298
299
300
301
}

static void fd_stop (fdrive_t *drv)
{
302
    drv->drflags &= ~FDRIVE_MOTOR_ON;
303
304
305
306
307
308
309
310
311
312
}

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

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

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);
332
333

enum {
334
    FD_CTRL_ACTIVE = 0x01, /* XXX: suppress that */
335
    FD_CTRL_RESET  = 0x02,
336
337
    FD_CTRL_SLEEP  = 0x04, /* XXX: suppress that */
    FD_CTRL_BUSY   = 0x08, /* dma transfer in progress */
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
    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,
356
    FD_STATE_FORMAT = 0x40,
357
358
359
};

#define FD_STATE(state) ((state) & FD_STATE_STATE)
360
361
#define FD_SET_STATE(state, new_state) \
do { (state) = ((state) & ~FD_STATE_STATE) | (new_state); } while (0)
362
363
#define FD_MULTI_TRACK(state) ((state) & FD_STATE_MULTI)
#define FD_DID_SEEK(state) ((state) & FD_STATE_SEEK)
364
#define FD_FORMAT_CMD(state) ((state) & FD_STATE_FORMAT)
365
366
367
struct fdctrl_t {
    fdctrl_t *fdctrl;
bellard authored
368
    /* Controller's identification */
369
370
371
372
    uint8_t version;
    /* HW */
    int irq_lvl;
    int dma_chann;
373
    uint32_t io_base;
bellard authored
374
    /* Controller state */
375
    QEMUTimer *result_timer;
376
377
378
379
380
381
382
383
384
385
386
    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;
387
    uint8_t eot; /* last wanted sector */
388
389
390
391
392
393
394
395
396
397
398
399
    /* 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;
    /* Floppy drives */
    fdrive_t drives[2];
400
401
402
403
404
405
406
};

static uint32_t fdctrl_read (void *opaque, uint32_t reg)
{
    fdctrl_t *fdctrl = opaque;
    uint32_t retval;
407
    switch (reg & 0x07) {
bellard authored
408
409
410
411
412
413
#ifdef TARGET_SPARC
    case 0x00:
	// Identify to Linux as S82078B
	retval = fdctrl_read_statusB(fdctrl);
	break;
#endif
414
    case 0x01:
415
	retval = fdctrl_read_statusB(fdctrl);
416
417
	break;
    case 0x02:
418
	retval = fdctrl_read_dor(fdctrl);
419
420
	break;
    case 0x03:
421
        retval = fdctrl_read_tape(fdctrl);
422
423
	break;
    case 0x04:
424
        retval = fdctrl_read_main_status(fdctrl);
425
426
	break;
    case 0x05:
427
        retval = fdctrl_read_data(fdctrl);
428
429
	break;
    case 0x07:
430
        retval = fdctrl_read_dir(fdctrl);
431
432
	break;
    default:
433
	retval = (uint32_t)(-1);
434
435
	break;
    }
436
    FLOPPY_DPRINTF("read reg%d: 0x%02x\n", reg & 7, retval);
437
438
439
440
441
442
443
444

    return retval;
}

static void fdctrl_write (void *opaque, uint32_t reg, uint32_t value)
{
    fdctrl_t *fdctrl = opaque;
445
446
    FLOPPY_DPRINTF("write reg%d: 0x%02x\n", reg & 7, value);
447
448
    switch (reg & 0x07) {
    case 0x02:
449
	fdctrl_write_dor(fdctrl, value);
450
451
	break;
    case 0x03:
452
        fdctrl_write_tape(fdctrl, value);
453
454
	break;
    case 0x04:
455
        fdctrl_write_rate(fdctrl, value);
456
457
	break;
    case 0x05:
458
        fdctrl_write_data(fdctrl, value);
459
460
461
462
	break;
    default:
	break;
    }
463
464
}
465
466
467
468
469
470
471
472
473
474
475
static uint32_t fdctrl_read_mem (void *opaque, target_phys_addr_t reg)
{
    return fdctrl_read(opaque, reg);
}

static void fdctrl_write_mem (void *opaque, 
                              target_phys_addr_t reg, uint32_t value)
{
    fdctrl_write(opaque, reg, value);
}
bellard authored
476
static CPUReadMemoryFunc *fdctrl_mem_read[3] = {
477
478
479
    fdctrl_read_mem,
    fdctrl_read_mem,
    fdctrl_read_mem,
bellard authored
480
481
482
};

static CPUWriteMemoryFunc *fdctrl_mem_write[3] = {
483
484
485
    fdctrl_write_mem,
    fdctrl_write_mem,
    fdctrl_write_mem,
bellard authored
486
487
};
488
489
490
fdctrl_t *fdctrl_init (int irq_lvl, int dma_chann, int mem_mapped, 
                       uint32_t io_base,
                       BlockDriverState **fds)
491
{
492
    fdctrl_t *fdctrl;
bellard authored
493
    int io_mem;
494
495
    int i;
bellard authored
496
    FLOPPY_DPRINTF("init controller\n");
497
498
499
    fdctrl = qemu_mallocz(sizeof(fdctrl_t));
    if (!fdctrl)
        return NULL;
500
501
502
    fdctrl->result_timer = qemu_new_timer(vm_clock, 
                                          fdctrl_result_timer, fdctrl);
bellard authored
503
    fdctrl->version = 0x90; /* Intel 82078 controller */
504
505
506
    fdctrl->irq_lvl = irq_lvl;
    fdctrl->dma_chann = dma_chann;
    fdctrl->io_base = io_base;
507
    fdctrl->config = 0x60; /* Implicit seek, polling & FIFO enabled */
508
509
510
    if (fdctrl->dma_chann != -1) {
        fdctrl->dma_en = 1;
        DMA_register_channel(dma_chann, &fdctrl_transfer_handler, fdctrl);
511
    } else {
512
        fdctrl->dma_en = 0;
513
    }
514
515
    for (i = 0; i < 2; i++) {
        fd_init(&fdctrl->drives[i], fds[i]);
516
    }
517
518
    fdctrl_reset(fdctrl, 0);
    fdctrl->state = FD_CTRL_ACTIVE;
519
    if (mem_mapped) {
bellard authored
520
521
        io_mem = cpu_register_io_memory(0, fdctrl_mem_read, fdctrl_mem_write, fdctrl);
        cpu_register_physical_memory(io_base, 0x08, io_mem);
522
    } else {
523
524
525
526
        register_ioport_read(io_base + 0x01, 5, 1, &fdctrl_read, fdctrl);
        register_ioport_read(io_base + 0x07, 1, 1, &fdctrl_read, fdctrl);
        register_ioport_write(io_base + 0x01, 5, 1, &fdctrl_write, fdctrl);
        register_ioport_write(io_base + 0x07, 1, 1, &fdctrl_write, fdctrl);
527
    }
528
    for (i = 0; i < 2; i++) {
529
        fd_revalidate(&fdctrl->drives[i]);
530
    }
531
532
    return fdctrl;
533
}
534
535
536
/* XXX: may change if moved to bdrv */
int fdctrl_get_drive_type(fdctrl_t *fdctrl, int drive_num)
537
{
538
    return fdctrl->drives[drive_num].drive;
539
540
541
}

/* Change IRQ state */
542
static void fdctrl_reset_irq (fdctrl_t *fdctrl)
543
{
544
545
546
    FLOPPY_DPRINTF("Reset interrupt\n");
    pic_set_irq(fdctrl->irq_lvl, 0);
    fdctrl->state &= ~FD_CTRL_INTR;
547
548
}
549
static void fdctrl_raise_irq (fdctrl_t *fdctrl, uint8_t status)
550
{
bellard authored
551
552
553
554
555
556
557
558
#ifdef TARGET_SPARC
    // Sparc mutation
    if (!fdctrl->dma_en) {
	fdctrl->state &= ~FD_CTRL_BUSY;
	fdctrl->int_status = status;
	return;
    }
#endif
559
560
561
    if (~(fdctrl->state & FD_CTRL_INTR)) {
        pic_set_irq(fdctrl->irq_lvl, 1);
        fdctrl->state |= FD_CTRL_INTR;
562
563
    }
    FLOPPY_DPRINTF("Set interrupt status to 0x%02x\n", status);
564
    fdctrl->int_status = status;
565
566
}
bellard authored
567
/* Reset controller */
568
static void fdctrl_reset (fdctrl_t *fdctrl, int do_irq)
569
570
571
{
    int i;
bellard authored
572
    FLOPPY_DPRINTF("reset controller\n");
573
    fdctrl_reset_irq(fdctrl);
bellard authored
574
    /* Initialise controller */
575
    fdctrl->cur_drv = 0;
576
    /* FIFO state */
577
578
579
580
    fdctrl->data_pos = 0;
    fdctrl->data_len = 0;
    fdctrl->data_state = FD_STATE_CMD;
    fdctrl->data_dir = FD_DIR_WRITE;
581
    for (i = 0; i < MAX_FD; i++)
582
583
        fd_reset(&fdctrl->drives[i]);
    fdctrl_reset_fifo(fdctrl);
584
    if (do_irq)
585
        fdctrl_raise_irq(fdctrl, 0xc0);
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
}

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);
601
602
603
}

/* Status B register : 0x01 (read-only) */
604
static uint32_t fdctrl_read_statusB (fdctrl_t *fdctrl)
605
606
607
608
609
610
{
    FLOPPY_DPRINTF("status register: 0x00\n");
    return 0;
}

/* Digital output register : 0x02 */
611
static uint32_t fdctrl_read_dor (fdctrl_t *fdctrl)
612
613
614
615
{
    uint32_t retval = 0;

    /* Drive motors state indicators */
616
617
618
619
    if (drv0(fdctrl)->drflags & FDRIVE_MOTOR_ON)
	retval |= 1 << 5;
    if (drv1(fdctrl)->drflags & FDRIVE_MOTOR_ON)
	retval |= 1 << 4;
620
    /* DMA enable */
621
    retval |= fdctrl->dma_en << 3;
622
    /* Reset indicator */
623
    retval |= (fdctrl->state & FD_CTRL_RESET) == 0 ? 0x04 : 0;
624
    /* Selected drive */
625
    retval |= fdctrl->cur_drv;
626
627
628
629
630
    FLOPPY_DPRINTF("digital output register: 0x%02x\n", retval);

    return retval;
}
631
static void fdctrl_write_dor (fdctrl_t *fdctrl, uint32_t value)
632
633
{
    /* Reset mode */
634
    if (fdctrl->state & FD_CTRL_RESET) {
635
        if (!(value & 0x04)) {
bellard authored
636
            FLOPPY_DPRINTF("Floppy controller in RESET state !\n");
637
638
639
640
641
642
            return;
        }
    }
    FLOPPY_DPRINTF("digital output register set to 0x%02x\n", value);
    /* Drive motors state indicators */
    if (value & 0x20)
643
        fd_start(drv1(fdctrl));
644
    else
645
        fd_stop(drv1(fdctrl));
646
    if (value & 0x10)
647
        fd_start(drv0(fdctrl));
648
    else
649
        fd_stop(drv0(fdctrl));
650
651
    /* DMA enable */
#if 0
652
653
    if (fdctrl->dma_chann != -1)
        fdctrl->dma_en = 1 - ((value >> 3) & 1);
654
655
656
#endif
    /* Reset */
    if (!(value & 0x04)) {
657
        if (!(fdctrl->state & FD_CTRL_RESET)) {
bellard authored
658
            FLOPPY_DPRINTF("controller enter RESET state\n");
659
            fdctrl->state |= FD_CTRL_RESET;
660
661
        }
    } else {
662
        if (fdctrl->state & FD_CTRL_RESET) {
bellard authored
663
            FLOPPY_DPRINTF("controller out of RESET state\n");
664
            fdctrl_reset(fdctrl, 1);
665
            fdctrl->state &= ~(FD_CTRL_RESET | FD_CTRL_SLEEP);
666
667
668
        }
    }
    /* Selected drive */
669
    fdctrl->cur_drv = value & 1;
670
671
672
}

/* Tape drive register : 0x03 */
673
static uint32_t fdctrl_read_tape (fdctrl_t *fdctrl)
674
675
676
677
{
    uint32_t retval = 0;

    /* Disk boot selection indicator */
678
    retval |= fdctrl->bootsel << 2;
679
680
681
682
683
684
    /* Tape indicators: never allowed */
    FLOPPY_DPRINTF("tape drive register: 0x%02x\n", retval);

    return retval;
}
685
static void fdctrl_write_tape (fdctrl_t *fdctrl, uint32_t value)
686
687
{
    /* Reset mode */
688
    if (fdctrl->state & FD_CTRL_RESET) {
bellard authored
689
        FLOPPY_DPRINTF("Floppy controller in RESET state !\n");
690
691
692
693
        return;
    }
    FLOPPY_DPRINTF("tape drive register set to 0x%02x\n", value);
    /* Disk boot selection indicator */
694
    fdctrl->bootsel = (value >> 2) & 1;
695
696
697
698
    /* Tape indicators: never allow */
}

/* Main status register : 0x04 (read) */
699
static uint32_t fdctrl_read_main_status (fdctrl_t *fdctrl)
700
701
702
{
    uint32_t retval = 0;
703
704
    fdctrl->state &= ~(FD_CTRL_SLEEP | FD_CTRL_RESET);
    if (!(fdctrl->state & FD_CTRL_BUSY)) {
705
706
707
        /* Data transfer allowed */
        retval |= 0x80;
        /* Data transfer direction indicator */
708
        if (fdctrl->data_dir == FD_DIR_READ)
709
710
711
712
            retval |= 0x40;
    }
    /* Should handle 0x20 for SPECIFY command */
    /* Command busy indicator */
713
714
    if (FD_STATE(fdctrl->data_state) == FD_STATE_DATA ||
        FD_STATE(fdctrl->data_state) == FD_STATE_STATUS)
715
716
717
718
719
720
721
        retval |= 0x10;
    FLOPPY_DPRINTF("main status register: 0x%02x\n", retval);

    return retval;
}

/* Data select rate register : 0x04 (write) */
722
static void fdctrl_write_rate (fdctrl_t *fdctrl, uint32_t value)
723
724
{
    /* Reset mode */
725
    if (fdctrl->state & FD_CTRL_RESET) {
bellard authored
726
            FLOPPY_DPRINTF("Floppy controller in RESET state !\n");
727
728
729
730
731
            return;
        }
    FLOPPY_DPRINTF("select rate register set to 0x%02x\n", value);
    /* Reset: autoclear */
    if (value & 0x80) {
732
733
734
        fdctrl->state |= FD_CTRL_RESET;
        fdctrl_reset(fdctrl, 1);
        fdctrl->state &= ~FD_CTRL_RESET;
735
736
    }
    if (value & 0x40) {
737
738
        fdctrl->state |= FD_CTRL_SLEEP;
        fdctrl_reset(fdctrl, 1);
739
740
741
742
    }
//        fdctrl.precomp = (value >> 2) & 0x07;
}
bellard authored
743
744
745
746
747
748
749
750
751
752
753
754
static int fdctrl_media_changed(fdrive_t *drv)
{
    int ret;
    if (!drv->bs) 
        return 0;
    ret = bdrv_media_changed(drv->bs);
    if (ret) {
        fd_revalidate(drv);
    }
    return ret;
}
755
/* Digital input register : 0x07 (read-only) */
756
static uint32_t fdctrl_read_dir (fdctrl_t *fdctrl)
757
758
759
{
    uint32_t retval = 0;
bellard authored
760
761
    if (fdctrl_media_changed(drv0(fdctrl)) ||
	fdctrl_media_changed(drv1(fdctrl)))
762
763
        retval |= 0x80;
    if (retval != 0)
764
        FLOPPY_DPRINTF("Floppy digital input register: 0x%02x\n", retval);
765
766
767
768
769

    return retval;
}

/* FIFO state control */
770
static void fdctrl_reset_fifo (fdctrl_t *fdctrl)
771
{
772
773
774
    fdctrl->data_dir = FD_DIR_WRITE;
    fdctrl->data_pos = 0;
    FD_SET_STATE(fdctrl->data_state, FD_STATE_CMD);
775
776
777
}

/* Set FIFO status for the host to read */
778
static void fdctrl_set_fifo (fdctrl_t *fdctrl, int fifo_len, int do_irq)
779
{
780
781
782
783
    fdctrl->data_dir = FD_DIR_READ;
    fdctrl->data_len = fifo_len;
    fdctrl->data_pos = 0;
    FD_SET_STATE(fdctrl->data_state, FD_STATE_STATUS);
784
    if (do_irq)
785
        fdctrl_raise_irq(fdctrl, 0x00);
786
787
788
}

/* Set an error: unimplemented/unknown command */
789
static void fdctrl_unimplemented (fdctrl_t *fdctrl)
790
791
{
#if 0
792
793
794
    fdrive_t *cur_drv;

    cur_drv = get_cur_drv(fdctrl);
795
    fdctrl->fifo[0] = 0x60 | (cur_drv->head << 2) | fdctrl->cur_drv;
796
797
798
    fdctrl->fifo[1] = 0x00;
    fdctrl->fifo[2] = 0x00;
    fdctrl_set_fifo(fdctrl, 3, 1);
799
#else
800
801
802
    //    fdctrl_reset_fifo(fdctrl);
    fdctrl->fifo[0] = 0x80;
    fdctrl_set_fifo(fdctrl, 1, 0);
803
804
805
806
#endif
}

/* Callback for transfer end (stop or abort) */
807
808
static void fdctrl_stop_transfer (fdctrl_t *fdctrl, uint8_t status0,
				  uint8_t status1, uint8_t status2)
809
{
810
    fdrive_t *cur_drv;
811
812
    cur_drv = get_cur_drv(fdctrl);
813
814
    FLOPPY_DPRINTF("transfer status: %02x %02x %02x (%02x)\n",
                   status0, status1, status2,
815
816
                   status0 | (cur_drv->head << 2) | fdctrl->cur_drv);
    fdctrl->fifo[0] = status0 | (cur_drv->head << 2) | fdctrl->cur_drv;
817
818
819
820
821
822
823
    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;
824
    if (fdctrl->state & FD_CTRL_BUSY) {
825
        DMA_release_DREQ(fdctrl->dma_chann);
826
827
        fdctrl->state &= ~FD_CTRL_BUSY;
    }
828
    fdctrl_set_fifo(fdctrl, 7, 1);
829
830
831
}

/* Prepare a data transfer (either DMA or FIFO) */
832
static void fdctrl_start_transfer (fdctrl_t *fdctrl, int direction)
833
{
834
    fdrive_t *cur_drv;
835
836
837
    uint8_t kh, kt, ks;
    int did_seek;
838
839
840
841
842
    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
843
    FLOPPY_DPRINTF("Start transfer at %d %d %02x %02x (%d)\n",
844
                   fdctrl->cur_drv, kh, kt, ks,
845
846
                   _fd_sector(kh, kt, ks, cur_drv->last_sect));
    did_seek = 0;
847
    switch (fd_seek(cur_drv, kh, kt, ks, fdctrl->config & 0x40)) {
848
849
    case 2:
        /* sect too big */
850
851
852
853
        fdctrl_stop_transfer(fdctrl, 0x40, 0x00, 0x00);
        fdctrl->fifo[3] = kt;
        fdctrl->fifo[4] = kh;
        fdctrl->fifo[5] = ks;
854
855
856
        return;
    case 3:
        /* track too big */
857
858
859
860
        fdctrl_stop_transfer(fdctrl, 0x40, 0x80, 0x00);
        fdctrl->fifo[3] = kt;
        fdctrl->fifo[4] = kh;
        fdctrl->fifo[5] = ks;
861
862
863
        return;
    case 4:
        /* No seek enabled */
864
865
866
867
        fdctrl_stop_transfer(fdctrl, 0x40, 0x00, 0x00);
        fdctrl->fifo[3] = kt;
        fdctrl->fifo[4] = kh;
        fdctrl->fifo[5] = ks;
868
869
870
871
872
873
874
875
        return;
    case 1:
        did_seek = 1;
        break;
    default:
        break;
    }
    /* Set the FIFO state */
876
877
878
879
880
881
882
    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;
883
    if (did_seek)
884
885
886
887
888
889
890
        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 {
	int tmp;
891
        fdctrl->data_len = 128 << (fdctrl->fifo[5] > 7 ? 7 : fdctrl->fifo[5]);
892
893
894
895
896
        tmp = (cur_drv->last_sect - ks + 1);
        if (fdctrl->fifo[0] & 0x80)
            tmp += cur_drv->last_sect;
	fdctrl->data_len *= tmp;
    }
897
    fdctrl->eot = fdctrl->fifo[6];
898
    if (fdctrl->dma_en) {
899
900
        int dma_mode;
        /* DMA transfer are enabled. Check if DMA channel is well programmed */
901
        dma_mode = DMA_get_channel_mode(fdctrl->dma_chann);
902
        dma_mode = (dma_mode >> 2) & 3;
903
904
905
906
        FLOPPY_DPRINTF("dma_mode=%d direction=%d (%d - %d)\n",
		       dma_mode, direction,
                       (128 << fdctrl->fifo[5]) *
		       (cur_drv->last_sect - ks + 1), fdctrl->data_len);
907
908
909
910
911
        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 */
912
            fdctrl->state |= FD_CTRL_BUSY;
bellard authored
913
            /* Now, we just have to wait for the DMA controller to
914
915
             * recall us...
             */
916
917
            DMA_hold_DREQ(fdctrl->dma_chann);
            DMA_schedule(fdctrl->dma_chann);
918
            return;
919
920
        } else {
	    FLOPPY_ERROR("dma_mode=%d direction=%d\n", dma_mode, direction);
921
922
923
924
        }
    }
    FLOPPY_DPRINTF("start non-DMA transfer\n");
    /* IO based transfer: calculate len */
925
    fdctrl_raise_irq(fdctrl, 0x00);
926
927
928
929
930

    return;
}

/* Prepare a transfer of deleted data */
931
static void fdctrl_start_transfer_del (fdctrl_t *fdctrl, int direction)
932
933
934
935
{
    /* We don't handle deleted data,
     * so we don't return *ANYTHING*
     */
936
    fdctrl_stop_transfer(fdctrl, 0x60, 0x00, 0x00);
937
938
939
}

/* handlers for DMA transfers */
bellard authored
940
941
static int fdctrl_transfer_handler (void *opaque, int nchan,
                                    int dma_pos, int dma_len)
942
{
943
944
945
    fdctrl_t *fdctrl;
    fdrive_t *cur_drv;
    int len, start_pos, rel_pos;
946
947
    uint8_t status0 = 0x00, status1 = 0x00, status2 = 0x00;
948
949
    fdctrl = opaque;
    if (!(fdctrl->state & FD_CTRL_BUSY)) {
950
951
952
        FLOPPY_DPRINTF("Not in DMA transfer mode !\n");
        return 0;
    }
953
954
955
    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)
956
        status2 = 0x04;
bellard authored
957
958
    if (dma_len > fdctrl->data_len)
        dma_len = fdctrl->data_len;
959
    if (cur_drv->bs == NULL) {
960
961
962
963
964
	if (fdctrl->data_dir == FD_DIR_WRITE)
	    fdctrl_stop_transfer(fdctrl, 0x60, 0x00, 0x00);
	else
	    fdctrl_stop_transfer(fdctrl, 0x40, 0x00, 0x00);
	len = 0;
965
966
        goto transfer_error;
    }
967
    rel_pos = fdctrl->data_pos % FD_SECTOR_LEN;
bellard authored
968
969
    for (start_pos = fdctrl->data_pos; fdctrl->data_pos < dma_len;) {
        len = dma_len - fdctrl->data_pos;
970
971
        if (len + rel_pos > FD_SECTOR_LEN)
            len = FD_SECTOR_LEN - rel_pos;
bellard authored
972
973
        FLOPPY_DPRINTF("copy %d bytes (%d %d %d) %d pos %d %02x "
                       "(%d-0x%08x 0x%08x)\n", len, dma_len, fdctrl->data_pos,
974
975
                       fdctrl->data_len, fdctrl->cur_drv, cur_drv->head,
                       cur_drv->track, cur_drv->sect, fd_sector(cur_drv),
bellard authored
976
                       fd_sector(cur_drv) * 512);
977
978
979
980
981
        if (fdctrl->data_dir != FD_DIR_WRITE ||
	    len < FD_SECTOR_LEN || rel_pos != 0) {
            /* READ & SCAN commands and realign to a sector for WRITE */
            if (bdrv_read(cur_drv->bs, fd_sector(cur_drv),
			  fdctrl->fifo, 1) < 0) {
982
983
984
                FLOPPY_DPRINTF("Floppy: error getting sector %d\n",
                               fd_sector(cur_drv));
                /* Sure, image size is too small... */
985
                memset(fdctrl->fifo, 0, FD_SECTOR_LEN);
986
            }
987
        }
988
989
990
	switch (fdctrl->data_dir) {
	case FD_DIR_READ:
	    /* READ commands */
bellard authored
991
992
993
994
            DMA_write_memory (nchan, fdctrl->fifo + rel_pos,
                              fdctrl->data_pos, len);
/* 	    cpu_physical_memory_write(addr + fdctrl->data_pos, */
/* 				      fdctrl->fifo + rel_pos, len); */
995
996
997
	    break;
	case FD_DIR_WRITE:
            /* WRITE commands */
bellard authored
998
999
1000
1001
            DMA_read_memory (nchan, fdctrl->fifo + rel_pos,
                             fdctrl->data_pos, len);
/*             cpu_physical_memory_read(addr + fdctrl->data_pos, */
/* 				     fdctrl->fifo + rel_pos, len); */
1002
1003
1004
1005
1006
            if (bdrv_write(cur_drv->bs, fd_sector(cur_drv),
			   fdctrl->fifo, 1) < 0) {
                FLOPPY_ERROR("writting sector %d\n", fd_sector(cur_drv));
                fdctrl_stop_transfer(fdctrl, 0x60, 0x00, 0x00);
                goto transfer_error;
1007
            }
1008
1009
1010
1011
1012
1013
	    break;
	default:
	    /* SCAN commands */
            {
		uint8_t tmpbuf[FD_SECTOR_LEN];
                int ret;
bellard authored
1014
1015
1016
                DMA_read_memory (nchan, tmpbuf, fdctrl->data_pos, len);
/*                 cpu_physical_memory_read(addr + fdctrl->data_pos, */
/*                                          tmpbuf, len); */
1017
                ret = memcmp(tmpbuf, fdctrl->fifo + rel_pos, len);
1018
1019
1020
1021
                if (ret == 0) {
                    status2 = 0x08;
                    goto end_transfer;
                }
1022
1023
                if ((ret < 0 && fdctrl->data_dir == FD_DIR_SCANL) ||
                    (ret > 0 && fdctrl->data_dir == FD_DIR_SCANH)) {
1024
1025
1026
1027
                    status2 = 0x00;
                    goto end_transfer;
                }
            }
1028
	    break;
1029
        }
1030
1031
1032
	fdctrl->data_pos += len;
	rel_pos = fdctrl->data_pos % FD_SECTOR_LEN;
        if (rel_pos == 0) {
1033
            /* Seek to next sector */
1034
1035
1036
	    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),
bellard authored
1037
			   fdctrl->data_pos - len);
1038
1039
1040
1041
            /* 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) {
1042
1043
1044
1045
		cur_drv->sect = 1;
		if (FD_MULTI_TRACK(fdctrl->data_state)) {
		    if (cur_drv->head == 0 &&
			(cur_drv->flags & FDISK_DBL_SIDES) != 0) {	
1046
1047
1048
                        cur_drv->head = 1;
                    } else {
                        cur_drv->head = 0;
1049
1050
1051
			cur_drv->track++;
			if ((cur_drv->flags & FDISK_DBL_SIDES) == 0)
			    break;
1052
1053
1054
1055
                    }
                } else {
                    cur_drv->track++;
                    break;
1056
                }
1057
1058
1059
		FLOPPY_DPRINTF("seek to next track (%d %02x %02x => %d)\n",
			       cur_drv->head, cur_drv->track,
			       cur_drv->sect, fd_sector(cur_drv));
1060
1061
            } else {
                cur_drv->sect++;
1062
1063
1064
1065
            }
        }
    }
end_transfer:
1066
1067
1068
1069
1070
1071
    len = fdctrl->data_pos - start_pos;
    FLOPPY_DPRINTF("end transfer %d %d %d\n",
		   fdctrl->data_pos, len, fdctrl->data_len);
    if (fdctrl->data_dir == FD_DIR_SCANE ||
        fdctrl->data_dir == FD_DIR_SCANL ||
        fdctrl->data_dir == FD_DIR_SCANH)
1072
        status2 = 0x08;
1073
    if (FD_DID_SEEK(fdctrl->data_state))
1074
        status0 |= 0x20;
1075
1076
    fdctrl->data_len -= len;
    //    if (fdctrl->data_len == 0)
1077
    fdctrl_stop_transfer(fdctrl, status0, status1, status2);
1078
1079
transfer_error:
1080
    return len;
1081
1082
1083
}

/* Data register : 0x05 */
1084
static uint32_t fdctrl_read_data (fdctrl_t *fdctrl)
1085
{
1086
    fdrive_t *cur_drv;
1087
1088
1089
    uint32_t retval = 0;
    int pos, len;
1090
1091
1092
    cur_drv = get_cur_drv(fdctrl);
    fdctrl->state &= ~FD_CTRL_SLEEP;
    if (FD_STATE(fdctrl->data_state) == FD_STATE_CMD) {
1093
1094
1095
        FLOPPY_ERROR("can't read data in CMD state\n");
        return 0;
    }
1096
1097
    pos = fdctrl->data_pos;
    if (FD_STATE(fdctrl->data_state) == FD_STATE_DATA) {
1098
1099
        pos %= FD_SECTOR_LEN;
        if (pos == 0) {
1100
            len = fdctrl->data_len - fdctrl->data_pos;
1101
1102
1103
            if (len > FD_SECTOR_LEN)
                len = FD_SECTOR_LEN;
            bdrv_read(cur_drv->bs, fd_sector(cur_drv),
1104
                      fdctrl->fifo, len);
1105
1106
        }
    }
1107
1108
1109
    retval = fdctrl->fifo[pos];
    if (++fdctrl->data_pos == fdctrl->data_len) {
        fdctrl->data_pos = 0;
1110
        /* Switch from transfer mode to status mode
1111
1112
         * then from status mode to command mode
         */
1113
        if (FD_STATE(fdctrl->data_state) == FD_STATE_DATA) {
1114
            fdctrl_stop_transfer(fdctrl, 0x20, 0x00, 0x00);
1115
        } else {
1116
            fdctrl_reset_fifo(fdctrl);
1117
1118
            fdctrl_reset_irq(fdctrl);
        }
1119
1120
1121
1122
1123
1124
    }
    FLOPPY_DPRINTF("data register: 0x%02x\n", retval);

    return retval;
}
1125
static void fdctrl_format_sector (fdctrl_t *fdctrl)
1126
{
1127
1128
1129
    fdrive_t *cur_drv;
    uint8_t kh, kt, ks;
    int did_seek;
1130
1131
1132
1133
1134
1135
1136
1137
1138
1139
1140
1141
1142
1143
1144
1145
1146
1147
1148
1149
1150
1151
1152
1153
1154
1155
1156
1157
1158
1159
1160
1161
1162
1163
1164
1165
1166
1167
1168
1169
1170
1171
1172
1173
1174
1175
1176
1177
1178
1179
1180
1181
1182
1183
1184
1185
1186
1187
1188
1189
1190
1191
1192
1193
1194
    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) {
        FLOPPY_ERROR("formating sector %d\n", fd_sector(cur_drv));
        fdctrl_stop_transfer(fdctrl, 0x60, 0x00, 0x00);
    } else {
	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;
	}
    }
}

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

    cur_drv = get_cur_drv(fdctrl);
1195
    /* Reset mode */
1196
    if (fdctrl->state & FD_CTRL_RESET) {
bellard authored
1197
        FLOPPY_DPRINTF("Floppy controller in RESET state !\n");
1198
1199
        return;
    }
1200
1201
    fdctrl->state &= ~FD_CTRL_SLEEP;
    if (FD_STATE(fdctrl->data_state) == FD_STATE_STATUS) {
1202
1203
1204
1205
        FLOPPY_ERROR("can't write data in status mode\n");
        return;
    }
    /* Is it write command time ? */
1206
    if (FD_STATE(fdctrl->data_state) == FD_STATE_DATA) {
1207
        /* FIFO data write */
1208
1209
1210
        fdctrl->fifo[fdctrl->data_pos++] = value;
        if (fdctrl->data_pos % FD_SECTOR_LEN == (FD_SECTOR_LEN - 1) ||
            fdctrl->data_pos == fdctrl->data_len) {
1211
            bdrv_write(cur_drv->bs, fd_sector(cur_drv),
1212
                       fdctrl->fifo, FD_SECTOR_LEN);
1213
        }
1214
        /* Switch from transfer mode to status mode
1215
1216
         * then from status mode to command mode
         */
1217
1218
        if (FD_STATE(fdctrl->data_state) == FD_STATE_DATA)
            fdctrl_stop_transfer(fdctrl, 0x20, 0x00, 0x00);
1219
1220
        return;
    }
1221
    if (fdctrl->data_pos == 0) {
1222
1223
1224
1225
1226
1227
        /* Command */
        switch (value & 0x5F) {
        case 0x46:
            /* READ variants */
            FLOPPY_DPRINTF("READ command\n");
            /* 8 parameters cmd */
1228
            fdctrl->data_len = 9;
1229
1230
1231
1232
1233
            goto enqueue;
        case 0x4C:
            /* READ_DELETED variants */
            FLOPPY_DPRINTF("READ_DELETED command\n");
            /* 8 parameters cmd */
1234
            fdctrl->data_len = 9;
1235
1236
1237
1238
1239
            goto enqueue;
        case 0x50:
            /* SCAN_EQUAL variants */
            FLOPPY_DPRINTF("SCAN_EQUAL command\n");
            /* 8 parameters cmd */
1240
            fdctrl->data_len = 9;
1241
1242
1243
1244
1245
            goto enqueue;
        case 0x56:
            /* VERIFY variants */
            FLOPPY_DPRINTF("VERIFY command\n");
            /* 8 parameters cmd */
1246
            fdctrl->data_len = 9;
1247
1248
1249
1250
1251
            goto enqueue;
        case 0x59:
            /* SCAN_LOW_OR_EQUAL variants */
            FLOPPY_DPRINTF("SCAN_LOW_OR_EQUAL command\n");
            /* 8 parameters cmd */
1252
            fdctrl->data_len = 9;
1253
1254
1255
1256
1257
            goto enqueue;
        case 0x5D:
            /* SCAN_HIGH_OR_EQUAL variants */
            FLOPPY_DPRINTF("SCAN_HIGH_OR_EQUAL command\n");
            /* 8 parameters cmd */
1258
            fdctrl->data_len = 9;
1259
1260
1261
1262
1263
1264
1265
1266
1267
            goto enqueue;
        default:
            break;
        }
        switch (value & 0x7F) {
        case 0x45:
            /* WRITE variants */
            FLOPPY_DPRINTF("WRITE command\n");
            /* 8 parameters cmd */
1268
            fdctrl->data_len = 9;
1269
1270
1271
1272
1273
            goto enqueue;
        case 0x49:
            /* WRITE_DELETED variants */
            FLOPPY_DPRINTF("WRITE_DELETED command\n");
            /* 8 parameters cmd */
1274
            fdctrl->data_len = 9;
1275
1276
1277
1278
1279
1280
1281
1282
1283
            goto enqueue;
        default:
            break;
        }
        switch (value) {
        case 0x03:
            /* SPECIFY */
            FLOPPY_DPRINTF("SPECIFY command\n");
            /* 1 parameter cmd */
1284
            fdctrl->data_len = 3;
1285
1286
1287
1288
1289
            goto enqueue;
        case 0x04:
            /* SENSE_DRIVE_STATUS */
            FLOPPY_DPRINTF("SENSE_DRIVE_STATUS command\n");
            /* 1 parameter cmd */
1290
            fdctrl->data_len = 2;
1291
1292
1293
1294
1295
            goto enqueue;
        case 0x07:
            /* RECALIBRATE */
            FLOPPY_DPRINTF("RECALIBRATE command\n");
            /* 1 parameter cmd */
1296
            fdctrl->data_len = 2;
1297
1298
1299
1300
            goto enqueue;
        case 0x08:
            /* SENSE_INTERRUPT_STATUS */
            FLOPPY_DPRINTF("SENSE_INTERRUPT_STATUS command (%02x)\n",
1301
                           fdctrl->int_status);
1302
            /* No parameters cmd: returns status if no interrupt */
bellard authored
1303
#if 0
1304
1305
            fdctrl->fifo[0] =
                fdctrl->int_status | (cur_drv->head << 2) | fdctrl->cur_drv;
bellard authored
1306
1307
1308
1309
1310
1311
1312
#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
1313
1314
1315
1316
            fdctrl->fifo[1] = cur_drv->track;
            fdctrl_set_fifo(fdctrl, 2, 0);
	    fdctrl_reset_irq(fdctrl);
	    fdctrl->int_status = 0xC0;
1317
1318
1319
1320
1321
            return;
        case 0x0E:
            /* DUMPREG */
            FLOPPY_DPRINTF("DUMPREG command\n");
            /* Drives position */
1322
1323
1324
1325
            fdctrl->fifo[0] = drv0(fdctrl)->track;
            fdctrl->fifo[1] = drv1(fdctrl)->track;
            fdctrl->fifo[2] = 0;
            fdctrl->fifo[3] = 0;
1326
            /* timers */
1327
1328
1329
1330
            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) |
1331
                    (cur_drv->perpendicular << 2);
1332
1333
1334
            fdctrl->fifo[8] = fdctrl->config;
            fdctrl->fifo[9] = fdctrl->precomp_trk;
            fdctrl_set_fifo(fdctrl, 10, 0);
1335
1336
1337
1338
1339
            return;
        case 0x0F:
            /* SEEK */
            FLOPPY_DPRINTF("SEEK command\n");
            /* 2 parameters cmd */
1340
            fdctrl->data_len = 3;
1341
1342
1343
1344
1345
            goto enqueue;
        case 0x10:
            /* VERSION */
            FLOPPY_DPRINTF("VERSION command\n");
            /* No parameters cmd */
bellard authored
1346
            /* Controller's version */
1347
1348
            fdctrl->fifo[0] = fdctrl->version;
            fdctrl_set_fifo(fdctrl, 1, 1);
1349
1350
1351
1352
1353
            return;
        case 0x12:
            /* PERPENDICULAR_MODE */
            FLOPPY_DPRINTF("PERPENDICULAR_MODE command\n");
            /* 1 parameter cmd */
1354
            fdctrl->data_len = 2;
1355
1356
1357
1358
1359
            goto enqueue;
        case 0x13:
            /* CONFIGURE */
            FLOPPY_DPRINTF("CONFIGURE command\n");
            /* 3 parameters cmd */
1360
            fdctrl->data_len = 4;
1361
1362
1363
1364
1365
            goto enqueue;
        case 0x14:
            /* UNLOCK */
            FLOPPY_DPRINTF("UNLOCK command\n");
            /* No parameters cmd */
1366
1367
1368
            fdctrl->lock = 0;
            fdctrl->fifo[0] = 0;
            fdctrl_set_fifo(fdctrl, 1, 0);
1369
1370
1371
1372
1373
            return;
        case 0x17:
            /* POWERDOWN_MODE */
            FLOPPY_DPRINTF("POWERDOWN_MODE command\n");
            /* 2 parameters cmd */
1374
            fdctrl->data_len = 3;
1375
1376
1377
1378
1379
            goto enqueue;
        case 0x18:
            /* PART_ID */
            FLOPPY_DPRINTF("PART_ID command\n");
            /* No parameters cmd */
1380
1381
            fdctrl->fifo[0] = 0x41; /* Stepping 1 */
            fdctrl_set_fifo(fdctrl, 1, 0);
1382
1383
1384
1385
1386
            return;
        case 0x2C:
            /* SAVE */
            FLOPPY_DPRINTF("SAVE command\n");
            /* No parameters cmd */
1387
1388
            fdctrl->fifo[0] = 0;
            fdctrl->fifo[1] = 0;
1389
            /* Drives position */
1390
1391
1392
1393
            fdctrl->fifo[2] = drv0(fdctrl)->track;
            fdctrl->fifo[3] = drv1(fdctrl)->track;
            fdctrl->fifo[4] = 0;
            fdctrl->fifo[5] = 0;
1394
            /* timers */
1395
1396
1397
1398
            fdctrl->fifo[6] = fdctrl->timer0;
            fdctrl->fifo[7] = fdctrl->timer1;
            fdctrl->fifo[8] = cur_drv->last_sect;
            fdctrl->fifo[9] = (fdctrl->lock << 7) |
1399
                    (cur_drv->perpendicular << 2);
1400
1401
1402
1403
1404
1405
            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);
1406
1407
1408
1409
1410
            return;
        case 0x33:
            /* OPTION */
            FLOPPY_DPRINTF("OPTION command\n");
            /* 1 parameter cmd */
1411
            fdctrl->data_len = 2;
1412
1413
1414
1415
1416
            goto enqueue;
        case 0x42:
            /* READ_TRACK */
            FLOPPY_DPRINTF("READ_TRACK command\n");
            /* 8 parameters cmd */
1417
            fdctrl->data_len = 9;
1418
1419
1420
1421
1422
            goto enqueue;
        case 0x4A:
            /* READ_ID */
            FLOPPY_DPRINTF("READ_ID command\n");
            /* 1 parameter cmd */
1423
            fdctrl->data_len = 2;
1424
1425
1426
1427
1428
            goto enqueue;
        case 0x4C:
            /* RESTORE */
            FLOPPY_DPRINTF("RESTORE command\n");
            /* 17 parameters cmd */
1429
            fdctrl->data_len = 18;
1430
1431
1432
1433
1434
            goto enqueue;
        case 0x4D:
            /* FORMAT_TRACK */
            FLOPPY_DPRINTF("FORMAT_TRACK command\n");
            /* 5 parameters cmd */
1435
            fdctrl->data_len = 6;
1436
1437
1438
1439
1440
            goto enqueue;
        case 0x8E:
            /* DRIVE_SPECIFICATION_COMMAND */
            FLOPPY_DPRINTF("DRIVE_SPECIFICATION_COMMAND command\n");
            /* 5 parameters cmd */
1441
            fdctrl->data_len = 6;
1442
1443
1444
1445
1446
            goto enqueue;
        case 0x8F:
            /* RELATIVE_SEEK_OUT */
            FLOPPY_DPRINTF("RELATIVE_SEEK_OUT command\n");
            /* 2 parameters cmd */
1447
            fdctrl->data_len = 3;
1448
1449
1450
1451
1452
            goto enqueue;
        case 0x94:
            /* LOCK */
            FLOPPY_DPRINTF("LOCK command\n");
            /* No parameters cmd */
1453
1454
1455
            fdctrl->lock = 1;
            fdctrl->fifo[0] = 0x10;
            fdctrl_set_fifo(fdctrl, 1, 1);
1456
1457
1458
1459
1460
            return;
        case 0xCD:
            /* FORMAT_AND_WRITE */
            FLOPPY_DPRINTF("FORMAT_AND_WRITE command\n");
            /* 10 parameters cmd */
1461
            fdctrl->data_len = 11;
1462
1463
1464
1465
1466
            goto enqueue;
        case 0xCF:
            /* RELATIVE_SEEK_IN */
            FLOPPY_DPRINTF("RELATIVE_SEEK_IN command\n");
            /* 2 parameters cmd */
1467
            fdctrl->data_len = 3;
1468
1469
1470
1471
            goto enqueue;
        default:
            /* Unknown command */
            FLOPPY_ERROR("unknown command: 0x%02x\n", value);
1472
            fdctrl_unimplemented(fdctrl);
1473
1474
1475
1476
            return;
        }
    }
enqueue:
1477
1478
1479
    FLOPPY_DPRINTF("%s: %02x\n", __func__, value);
    fdctrl->fifo[fdctrl->data_pos] = value;
    if (++fdctrl->data_pos == fdctrl->data_len) {
1480
1481
1482
        /* We now have all parameters
         * and will be able to treat the command
         */
1483
1484
1485
1486
1487
	if (fdctrl->data_state & FD_STATE_FORMAT) {
	    fdctrl_format_sector(fdctrl);
	    return;
	}
        switch (fdctrl->fifo[0] & 0x1F) {
1488
1489
1490
1491
        case 0x06:
        {
            /* READ variants */
            FLOPPY_DPRINTF("treat READ command\n");
1492
            fdctrl_start_transfer(fdctrl, FD_DIR_READ);
1493
1494
1495
1496
1497
1498
            return;
        }
        case 0x0C:
            /* READ_DELETED variants */
//            FLOPPY_DPRINTF("treat READ_DELETED command\n");
            FLOPPY_ERROR("treat READ_DELETED command\n");
1499
            fdctrl_start_transfer_del(fdctrl, FD_DIR_READ);
1500
1501
1502
1503
1504
            return;
        case 0x16:
            /* VERIFY variants */
//            FLOPPY_DPRINTF("treat VERIFY command\n");
            FLOPPY_ERROR("treat VERIFY command\n");
1505
            fdctrl_stop_transfer(fdctrl, 0x20, 0x00, 0x00);
1506
1507
1508
1509
1510
            return;
        case 0x10:
            /* SCAN_EQUAL variants */
//            FLOPPY_DPRINTF("treat SCAN_EQUAL command\n");
            FLOPPY_ERROR("treat SCAN_EQUAL command\n");
1511
            fdctrl_start_transfer(fdctrl, FD_DIR_SCANE);
1512
1513
1514
1515
1516
            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");
1517
            fdctrl_start_transfer(fdctrl, FD_DIR_SCANL);
1518
1519
1520
1521
1522
            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");
1523
            fdctrl_start_transfer(fdctrl, FD_DIR_SCANH);
1524
1525
1526
1527
            return;
        default:
            break;
        }
1528
        switch (fdctrl->fifo[0] & 0x3F) {
1529
1530
        case 0x05:
            /* WRITE variants */
1531
1532
            FLOPPY_DPRINTF("treat WRITE command (%02x)\n", fdctrl->fifo[0]);
            fdctrl_start_transfer(fdctrl, FD_DIR_WRITE);
1533
1534
1535
1536
1537
            return;
        case 0x09:
            /* WRITE_DELETED variants */
//            FLOPPY_DPRINTF("treat WRITE_DELETED command\n");
            FLOPPY_ERROR("treat WRITE_DELETED command\n");
1538
            fdctrl_start_transfer_del(fdctrl, FD_DIR_WRITE);
1539
1540
1541
1542
            return;
        default:
            break;
        }
1543
        switch (fdctrl->fifo[0]) {
1544
1545
1546
        case 0x03:
            /* SPECIFY */
            FLOPPY_DPRINTF("treat SPECIFY command\n");
1547
            fdctrl->timer0 = (fdctrl->fifo[1] >> 4) & 0xF;
1548
            fdctrl->timer1 = fdctrl->fifo[2] >> 1;
1549
	    fdctrl->dma_en = 1 - (fdctrl->fifo[2] & 1) ;
1550
            /* No result back */
1551
            fdctrl_reset_fifo(fdctrl);
1552
1553
1554
1555
            break;
        case 0x04:
            /* SENSE_DRIVE_STATUS */
            FLOPPY_DPRINTF("treat SENSE_DRIVE_STATUS command\n");
1556
1557
1558
            fdctrl->cur_drv = fdctrl->fifo[1] & 1;
	    cur_drv = get_cur_drv(fdctrl);
            cur_drv->head = (fdctrl->fifo[1] >> 2) & 1;
1559
            /* 1 Byte status back */
1560
            fdctrl->fifo[0] = (cur_drv->ro << 6) |
1561
                (cur_drv->track == 0 ? 0x10 : 0x00) |
1562
1563
1564
                (cur_drv->head << 2) |
                fdctrl->cur_drv |
                0x28;
1565
            fdctrl_set_fifo(fdctrl, 1, 0);
1566
1567
1568
1569
            break;
        case 0x07:
            /* RECALIBRATE */
            FLOPPY_DPRINTF("treat RECALIBRATE command\n");
1570
1571
            fdctrl->cur_drv = fdctrl->fifo[1] & 1;
	    cur_drv = get_cur_drv(fdctrl);
1572
            fd_recalibrate(cur_drv);
1573
	    fdctrl_reset_fifo(fdctrl);
1574
            /* Raise Interrupt */
1575
	    fdctrl_raise_irq(fdctrl, 0x20);
1576
1577
1578
1579
            break;
        case 0x0F:
            /* SEEK */
            FLOPPY_DPRINTF("treat SEEK command\n");
1580
1581
1582
1583
            fdctrl->cur_drv = fdctrl->fifo[1] & 1;
	    cur_drv = get_cur_drv(fdctrl);
	    fd_start(cur_drv);
            if (fdctrl->fifo[2] <= cur_drv->track)
1584
1585
1586
                cur_drv->dir = 1;
            else
                cur_drv->dir = 0;
1587
1588
1589
	    fdctrl_reset_fifo(fdctrl);
            if (fdctrl->fifo[2] > cur_drv->max_track) {
                fdctrl_raise_irq(fdctrl, 0x60);
1590
            } else {
1591
                cur_drv->track = fdctrl->fifo[2];
1592
                /* Raise Interrupt */
1593
                fdctrl_raise_irq(fdctrl, 0x20);
1594
1595
1596
1597
1598
            }
            break;
        case 0x12:
            /* PERPENDICULAR_MODE */
            FLOPPY_DPRINTF("treat PERPENDICULAR_MODE command\n");
1599
1600
            if (fdctrl->fifo[1] & 0x80)
                cur_drv->perpendicular = fdctrl->fifo[1] & 0x7;
1601
            /* No result back */
1602
            fdctrl_reset_fifo(fdctrl);
1603
1604
1605
1606
            break;
        case 0x13:
            /* CONFIGURE */
            FLOPPY_DPRINTF("treat CONFIGURE command\n");
1607
1608
            fdctrl->config = fdctrl->fifo[2];
            fdctrl->precomp_trk =  fdctrl->fifo[3];
1609
            /* No result back */
1610
            fdctrl_reset_fifo(fdctrl);
1611
1612
1613
1614
            break;
        case 0x17:
            /* POWERDOWN_MODE */
            FLOPPY_DPRINTF("treat POWERDOWN_MODE command\n");
1615
1616
1617
            fdctrl->pwrd = fdctrl->fifo[1];
            fdctrl->fifo[0] = fdctrl->fifo[1];
            fdctrl_set_fifo(fdctrl, 1, 1);
1618
1619
1620
1621
1622
            break;
        case 0x33:
            /* OPTION */
            FLOPPY_DPRINTF("treat OPTION command\n");
            /* No result back */
1623
            fdctrl_reset_fifo(fdctrl);
1624
1625
1626
1627
1628
            break;
        case 0x42:
            /* READ_TRACK */
//            FLOPPY_DPRINTF("treat READ_TRACK command\n");
            FLOPPY_ERROR("treat READ_TRACK command\n");
1629
            fdctrl_start_transfer(fdctrl, FD_DIR_READ);
1630
1631
1632
            break;
        case 0x4A:
                /* READ_ID */
1633
            FLOPPY_DPRINTF("treat READ_ID command\n");
1634
            /* XXX: should set main status register to busy */
1635
            cur_drv->head = (fdctrl->fifo[1] >> 2) & 1;
1636
1637
            qemu_mod_timer(fdctrl->result_timer, 
                           qemu_get_clock(vm_clock) + (ticks_per_sec / 50));
1638
1639
1640
1641
1642
            break;
        case 0x4C:
            /* RESTORE */
            FLOPPY_DPRINTF("treat RESTORE command\n");
            /* Drives position */
1643
1644
            drv0(fdctrl)->track = fdctrl->fifo[3];
            drv1(fdctrl)->track = fdctrl->fifo[4];
1645
            /* timers */
1646
1647
1648
1649
1650
1651
1652
1653
1654
            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);
1655
1656
1657
            break;
        case 0x4D:
            /* FORMAT_TRACK */
1658
1659
1660
1661
1662
1663
1664
1665
1666
1667
1668
1669
1670
1671
1672
1673
1674
1675
	    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];
#if 0
	    cur_drv->last_sect =
		cur_drv->flags & FDISK_DBL_SIDES ? fdctrl->fifo[3] :
		fdctrl->fifo[3] / 2;
#else
	    cur_drv->last_sect = fdctrl->fifo[3];
#endif
ths authored
1676
1677
1678
	    /* 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
1679
1680
1681
	     */
	    fdctrl->data_state &= ~FD_STATE_FORMAT;
	    fdctrl_stop_transfer(fdctrl, 0x00, 0x00, 0x00);
1682
1683
1684
1685
            break;
        case 0x8E:
            /* DRIVE_SPECIFICATION_COMMAND */
            FLOPPY_DPRINTF("treat DRIVE_SPECIFICATION_COMMAND command\n");
1686
            if (fdctrl->fifo[fdctrl->data_pos - 1] & 0x80) {
1687
                /* Command parameters done */
1688
1689
1690
1691
1692
                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);
1693
                } else {
1694
                    fdctrl_reset_fifo(fdctrl);
1695
                }
1696
            } else if (fdctrl->data_len > 7) {
1697
                /* ERROR */
1698
1699
1700
                fdctrl->fifo[0] = 0x80 |
                    (cur_drv->head << 2) | fdctrl->cur_drv;
                fdctrl_set_fifo(fdctrl, 1, 1);
1701
1702
1703
1704
1705
            }
            break;
        case 0x8F:
            /* RELATIVE_SEEK_OUT */
            FLOPPY_DPRINTF("treat RELATIVE_SEEK_OUT command\n");
1706
1707
1708
            fdctrl->cur_drv = fdctrl->fifo[1] & 1;
	    cur_drv = get_cur_drv(fdctrl);
	    fd_start(cur_drv);
1709
                cur_drv->dir = 0;
1710
1711
1712
1713
            if (fdctrl->fifo[2] + cur_drv->track >= cur_drv->max_track) {
		cur_drv->track = cur_drv->max_track - 1;
            } else {
                cur_drv->track += fdctrl->fifo[2];
1714
            }
1715
1716
	    fdctrl_reset_fifo(fdctrl);
	    fdctrl_raise_irq(fdctrl, 0x20);
1717
1718
1719
1720
1721
            break;
        case 0xCD:
            /* FORMAT_AND_WRITE */
//                FLOPPY_DPRINTF("treat FORMAT_AND_WRITE command\n");
            FLOPPY_ERROR("treat FORMAT_AND_WRITE command\n");
1722
            fdctrl_unimplemented(fdctrl);
1723
1724
1725
1726
            break;
        case 0xCF:
                /* RELATIVE_SEEK_IN */
            FLOPPY_DPRINTF("treat RELATIVE_SEEK_IN command\n");
1727
1728
1729
            fdctrl->cur_drv = fdctrl->fifo[1] & 1;
	    cur_drv = get_cur_drv(fdctrl);
	    fd_start(cur_drv);
1730
                cur_drv->dir = 1;
1731
1732
1733
1734
            if (fdctrl->fifo[2] > cur_drv->track) {
		cur_drv->track = 0;
            } else {
                cur_drv->track -= fdctrl->fifo[2];
1735
            }
1736
1737
1738
	    fdctrl_reset_fifo(fdctrl);
	    /* Raise Interrupt */
	    fdctrl_raise_irq(fdctrl, 0x20);
1739
1740
1741
1742
            break;
        }
    }
}
1743
1744
1745
1746
1747
1748

static void fdctrl_result_timer(void *opaque)
{
    fdctrl_t *fdctrl = opaque;
    fdctrl_stop_transfer(fdctrl, 0x00, 0x00, 0x00);
}