Blame view

hw/fdc.c 56.3 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
75
typedef enum fdrive_flags_t {
    FDRIVE_MOTOR_ON   = 0x01, /* motor on/off           */
    FDRIVE_REVALIDATE = 0x02, /* Revalidated            */
} fdrive_flags_t;

typedef enum fdisk_flags_t {
    FDISK_DBL_SIDES  = 0x01,
} fdisk_flags_t;
76
77
78
79
typedef struct fdrive_t {
    BlockDriverState *bs;
    /* Drive status */
    fdrive_type_t drive;
80
    fdrive_flags_t drflags;
81
82
83
84
85
86
87
88
89
    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 */
90
    fdisk_flags_t flags;
91
92
    uint8_t last_sect;        /* Nb sector per track    */
    uint8_t max_track;        /* Nb of tracks           */
93
    uint16_t bps;             /* Bytes per sector       */
94
95
96
    uint8_t ro;               /* Is read-only           */
} fdrive_t;
97
static void fd_init (fdrive_t *drv, BlockDriverState *bs)
98
99
{
    /* Drive */
100
    drv->bs = bs;
bellard authored
101
    drv->drive = FDRIVE_DRV_NONE;
102
    drv->drflags = 0;
103
104
    drv->perpendicular = 0;
    /* Disk */
105
    drv->last_sect = 0;
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
    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;
125
126
127
128
    int ret;

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

/* 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;
}
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
229
/* 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, },
};
230
/* Revalidate a disk drive after a disk change */
231
static void fd_revalidate (fdrive_t *drv)
232
{
233
234
235
    fd_format_t *parse;
    int64_t nb_sectors, size;
    int i, first_match, match;
236
    int nb_heads, max_track, last_sect, ro;
237
238

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

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

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

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

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);
335
336

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

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

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

    return retval;
}

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

static CPUWriteMemoryFunc *fdctrl_mem_write[3] = {
486
487
488
    fdctrl_write_mem,
    fdctrl_write_mem,
    fdctrl_write_mem,
bellard authored
489
490
};
491
492
493
static void fd_change_cb (void *opaque)
{
    fdrive_t *drv = opaque;
494
495
496
497
498
499
500
501
502
    FLOPPY_DPRINTF("disk change\n");
    fd_revalidate(drv);
#if 0
    fd_recalibrate(drv);
    fdctrl_reset_fifo(drv->fdctrl);
    fdctrl_raise_irq(drv->fdctrl, 0x20);
#endif
}
503
504
505
506
fdctrl_t *fdctrl_init (int irq_lvl, int dma_chann, int mem_mapped, 
                       uint32_t io_base,
                       BlockDriverState **fds)
507
{
508
    fdctrl_t *fdctrl;
bellard authored
509
    int io_mem;
510
511
    int i;
bellard authored
512
    FLOPPY_DPRINTF("init controller\n");
513
514
515
    fdctrl = qemu_mallocz(sizeof(fdctrl_t));
    if (!fdctrl)
        return NULL;
516
517
518
    fdctrl->result_timer = qemu_new_timer(vm_clock, 
                                          fdctrl_result_timer, fdctrl);
bellard authored
519
    fdctrl->version = 0x90; /* Intel 82078 controller */
520
521
522
    fdctrl->irq_lvl = irq_lvl;
    fdctrl->dma_chann = dma_chann;
    fdctrl->io_base = io_base;
523
    fdctrl->config = 0x60; /* Implicit seek, polling & FIFO enabled */
524
525
526
    if (fdctrl->dma_chann != -1) {
        fdctrl->dma_en = 1;
        DMA_register_channel(dma_chann, &fdctrl_transfer_handler, fdctrl);
527
    } else {
528
        fdctrl->dma_en = 0;
529
    }
530
531
532
533
534
535
    for (i = 0; i < 2; i++) {
        fd_init(&fdctrl->drives[i], fds[i]);
        if (fds[i]) {
            bdrv_set_change_cb(fds[i],
                               &fd_change_cb, &fdctrl->drives[i]);
        }
536
    }
537
538
    fdctrl_reset(fdctrl, 0);
    fdctrl->state = FD_CTRL_ACTIVE;
539
    if (mem_mapped) {
bellard authored
540
541
        io_mem = cpu_register_io_memory(0, fdctrl_mem_read, fdctrl_mem_write, fdctrl);
        cpu_register_physical_memory(io_base, 0x08, io_mem);
542
    } else {
543
544
545
546
        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);
547
    }
548
    for (i = 0; i < 2; i++) {
549
        fd_revalidate(&fdctrl->drives[i]);
550
    }
551
552
    return fdctrl;
553
}
554
555
556
/* XXX: may change if moved to bdrv */
int fdctrl_get_drive_type(fdctrl_t *fdctrl, int drive_num)
557
{
558
    return fdctrl->drives[drive_num].drive;
559
560
561
}

/* Change IRQ state */
562
static void fdctrl_reset_irq (fdctrl_t *fdctrl)
563
{
564
565
566
    FLOPPY_DPRINTF("Reset interrupt\n");
    pic_set_irq(fdctrl->irq_lvl, 0);
    fdctrl->state &= ~FD_CTRL_INTR;
567
568
}
569
static void fdctrl_raise_irq (fdctrl_t *fdctrl, uint8_t status)
570
{
bellard authored
571
572
573
574
575
576
577
578
#ifdef TARGET_SPARC
    // Sparc mutation
    if (!fdctrl->dma_en) {
	fdctrl->state &= ~FD_CTRL_BUSY;
	fdctrl->int_status = status;
	return;
    }
#endif
579
580
581
    if (~(fdctrl->state & FD_CTRL_INTR)) {
        pic_set_irq(fdctrl->irq_lvl, 1);
        fdctrl->state |= FD_CTRL_INTR;
582
583
    }
    FLOPPY_DPRINTF("Set interrupt status to 0x%02x\n", status);
584
    fdctrl->int_status = status;
585
586
}
bellard authored
587
/* Reset controller */
588
static void fdctrl_reset (fdctrl_t *fdctrl, int do_irq)
589
590
591
{
    int i;
bellard authored
592
    FLOPPY_DPRINTF("reset controller\n");
593
    fdctrl_reset_irq(fdctrl);
bellard authored
594
    /* Initialise controller */
595
    fdctrl->cur_drv = 0;
596
    /* FIFO state */
597
598
599
600
    fdctrl->data_pos = 0;
    fdctrl->data_len = 0;
    fdctrl->data_state = FD_STATE_CMD;
    fdctrl->data_dir = FD_DIR_WRITE;
601
    for (i = 0; i < MAX_FD; i++)
602
603
        fd_reset(&fdctrl->drives[i]);
    fdctrl_reset_fifo(fdctrl);
604
    if (do_irq)
605
        fdctrl_raise_irq(fdctrl, 0xc0);
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
}

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);
621
622
623
}

/* Status B register : 0x01 (read-only) */
624
static uint32_t fdctrl_read_statusB (fdctrl_t *fdctrl)
625
626
627
628
629
630
{
    FLOPPY_DPRINTF("status register: 0x00\n");
    return 0;
}

/* Digital output register : 0x02 */
631
static uint32_t fdctrl_read_dor (fdctrl_t *fdctrl)
632
633
634
635
{
    uint32_t retval = 0;

    /* Drive motors state indicators */
636
637
638
639
    if (drv0(fdctrl)->drflags & FDRIVE_MOTOR_ON)
	retval |= 1 << 5;
    if (drv1(fdctrl)->drflags & FDRIVE_MOTOR_ON)
	retval |= 1 << 4;
640
    /* DMA enable */
641
    retval |= fdctrl->dma_en << 3;
642
    /* Reset indicator */
643
    retval |= (fdctrl->state & FD_CTRL_RESET) == 0 ? 0x04 : 0;
644
    /* Selected drive */
645
    retval |= fdctrl->cur_drv;
646
647
648
649
650
    FLOPPY_DPRINTF("digital output register: 0x%02x\n", retval);

    return retval;
}
651
static void fdctrl_write_dor (fdctrl_t *fdctrl, uint32_t value)
652
653
{
    /* Reset mode */
654
    if (fdctrl->state & FD_CTRL_RESET) {
655
        if (!(value & 0x04)) {
bellard authored
656
            FLOPPY_DPRINTF("Floppy controller in RESET state !\n");
657
658
659
660
661
662
            return;
        }
    }
    FLOPPY_DPRINTF("digital output register set to 0x%02x\n", value);
    /* Drive motors state indicators */
    if (value & 0x20)
663
        fd_start(drv1(fdctrl));
664
    else
665
        fd_stop(drv1(fdctrl));
666
    if (value & 0x10)
667
        fd_start(drv0(fdctrl));
668
    else
669
        fd_stop(drv0(fdctrl));
670
671
    /* DMA enable */
#if 0
672
673
    if (fdctrl->dma_chann != -1)
        fdctrl->dma_en = 1 - ((value >> 3) & 1);
674
675
676
#endif
    /* Reset */
    if (!(value & 0x04)) {
677
        if (!(fdctrl->state & FD_CTRL_RESET)) {
bellard authored
678
            FLOPPY_DPRINTF("controller enter RESET state\n");
679
            fdctrl->state |= FD_CTRL_RESET;
680
681
        }
    } else {
682
        if (fdctrl->state & FD_CTRL_RESET) {
bellard authored
683
            FLOPPY_DPRINTF("controller out of RESET state\n");
684
            fdctrl_reset(fdctrl, 1);
685
            fdctrl->state &= ~(FD_CTRL_RESET | FD_CTRL_SLEEP);
686
687
688
        }
    }
    /* Selected drive */
689
    fdctrl->cur_drv = value & 1;
690
691
692
}

/* Tape drive register : 0x03 */
693
static uint32_t fdctrl_read_tape (fdctrl_t *fdctrl)
694
695
696
697
{
    uint32_t retval = 0;

    /* Disk boot selection indicator */
698
    retval |= fdctrl->bootsel << 2;
699
700
701
702
703
704
    /* Tape indicators: never allowed */
    FLOPPY_DPRINTF("tape drive register: 0x%02x\n", retval);

    return retval;
}
705
static void fdctrl_write_tape (fdctrl_t *fdctrl, uint32_t value)
706
707
{
    /* Reset mode */
708
    if (fdctrl->state & FD_CTRL_RESET) {
bellard authored
709
        FLOPPY_DPRINTF("Floppy controller in RESET state !\n");
710
711
712
713
        return;
    }
    FLOPPY_DPRINTF("tape drive register set to 0x%02x\n", value);
    /* Disk boot selection indicator */
714
    fdctrl->bootsel = (value >> 2) & 1;
715
716
717
718
    /* Tape indicators: never allow */
}

/* Main status register : 0x04 (read) */
719
static uint32_t fdctrl_read_main_status (fdctrl_t *fdctrl)
720
721
722
{
    uint32_t retval = 0;
723
724
    fdctrl->state &= ~(FD_CTRL_SLEEP | FD_CTRL_RESET);
    if (!(fdctrl->state & FD_CTRL_BUSY)) {
725
726
727
        /* Data transfer allowed */
        retval |= 0x80;
        /* Data transfer direction indicator */
728
        if (fdctrl->data_dir == FD_DIR_READ)
729
730
731
732
            retval |= 0x40;
    }
    /* Should handle 0x20 for SPECIFY command */
    /* Command busy indicator */
733
734
    if (FD_STATE(fdctrl->data_state) == FD_STATE_DATA ||
        FD_STATE(fdctrl->data_state) == FD_STATE_STATUS)
735
736
737
738
739
740
741
        retval |= 0x10;
    FLOPPY_DPRINTF("main status register: 0x%02x\n", retval);

    return retval;
}

/* Data select rate register : 0x04 (write) */
742
static void fdctrl_write_rate (fdctrl_t *fdctrl, uint32_t value)
743
744
{
    /* Reset mode */
745
    if (fdctrl->state & FD_CTRL_RESET) {
bellard authored
746
            FLOPPY_DPRINTF("Floppy controller in RESET state !\n");
747
748
749
750
751
            return;
        }
    FLOPPY_DPRINTF("select rate register set to 0x%02x\n", value);
    /* Reset: autoclear */
    if (value & 0x80) {
752
753
754
        fdctrl->state |= FD_CTRL_RESET;
        fdctrl_reset(fdctrl, 1);
        fdctrl->state &= ~FD_CTRL_RESET;
755
756
    }
    if (value & 0x40) {
757
758
        fdctrl->state |= FD_CTRL_SLEEP;
        fdctrl_reset(fdctrl, 1);
759
760
761
762
763
    }
//        fdctrl.precomp = (value >> 2) & 0x07;
}

/* Digital input register : 0x07 (read-only) */
764
static uint32_t fdctrl_read_dir (fdctrl_t *fdctrl)
765
766
767
{
    uint32_t retval = 0;
768
769
    if (drv0(fdctrl)->drflags & FDRIVE_REVALIDATE ||
	drv1(fdctrl)->drflags & FDRIVE_REVALIDATE)
770
771
        retval |= 0x80;
    if (retval != 0)
772
773
774
        FLOPPY_DPRINTF("Floppy digital input register: 0x%02x\n", retval);
    drv0(fdctrl)->drflags &= ~FDRIVE_REVALIDATE;
    drv1(fdctrl)->drflags &= ~FDRIVE_REVALIDATE;
775
776
777
778
779

    return retval;
}

/* FIFO state control */
780
static void fdctrl_reset_fifo (fdctrl_t *fdctrl)
781
{
782
783
784
    fdctrl->data_dir = FD_DIR_WRITE;
    fdctrl->data_pos = 0;
    FD_SET_STATE(fdctrl->data_state, FD_STATE_CMD);
785
786
787
}

/* Set FIFO status for the host to read */
788
static void fdctrl_set_fifo (fdctrl_t *fdctrl, int fifo_len, int do_irq)
789
{
790
791
792
793
    fdctrl->data_dir = FD_DIR_READ;
    fdctrl->data_len = fifo_len;
    fdctrl->data_pos = 0;
    FD_SET_STATE(fdctrl->data_state, FD_STATE_STATUS);
794
    if (do_irq)
795
        fdctrl_raise_irq(fdctrl, 0x00);
796
797
798
}

/* Set an error: unimplemented/unknown command */
799
static void fdctrl_unimplemented (fdctrl_t *fdctrl)
800
801
{
#if 0
802
803
804
    fdrive_t *cur_drv;

    cur_drv = get_cur_drv(fdctrl);
805
    fdctrl->fifo[0] = 0x60 | (cur_drv->head << 2) | fdctrl->cur_drv;
806
807
808
    fdctrl->fifo[1] = 0x00;
    fdctrl->fifo[2] = 0x00;
    fdctrl_set_fifo(fdctrl, 3, 1);
809
#else
810
811
812
    //    fdctrl_reset_fifo(fdctrl);
    fdctrl->fifo[0] = 0x80;
    fdctrl_set_fifo(fdctrl, 1, 0);
813
814
815
816
#endif
}

/* Callback for transfer end (stop or abort) */
817
818
static void fdctrl_stop_transfer (fdctrl_t *fdctrl, uint8_t status0,
				  uint8_t status1, uint8_t status2)
819
{
820
    fdrive_t *cur_drv;
821
822
    cur_drv = get_cur_drv(fdctrl);
823
824
    FLOPPY_DPRINTF("transfer status: %02x %02x %02x (%02x)\n",
                   status0, status1, status2,
825
826
                   status0 | (cur_drv->head << 2) | fdctrl->cur_drv);
    fdctrl->fifo[0] = status0 | (cur_drv->head << 2) | fdctrl->cur_drv;
827
828
829
830
831
832
833
    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;
834
    if (fdctrl->state & FD_CTRL_BUSY) {
835
        DMA_release_DREQ(fdctrl->dma_chann);
836
837
        fdctrl->state &= ~FD_CTRL_BUSY;
    }
838
    fdctrl_set_fifo(fdctrl, 7, 1);
839
840
841
}

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

    return;
}

/* Prepare a transfer of deleted data */
941
static void fdctrl_start_transfer_del (fdctrl_t *fdctrl, int direction)
942
943
944
945
{
    /* We don't handle deleted data,
     * so we don't return *ANYTHING*
     */
946
    fdctrl_stop_transfer(fdctrl, 0x60, 0x00, 0x00);
947
948
949
}

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

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

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

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