fdc.c 54.4 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
/*
 * QEMU Floppy disk emulator
 * 
 * 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.
 */
#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            */
52
53
    FDRIVE_DISK_USER  = 0x04, /* User defined geometry  */
    FDRIVE_DISK_NONE  = 0x05, /* No disk                */
54
55
56
57
58
59
60
61
62
} 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;

63
64
65
66
67
68
69
70
71
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;

72
73
74
75
typedef struct fdrive_t {
    BlockDriverState *bs;
    /* Drive status */
    fdrive_type_t drive;
76
    fdrive_flags_t drflags;
77
78
79
80
81
82
83
84
85
    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 */
86
    fdisk_flags_t flags;
87
88
    uint8_t last_sect;        /* Nb sector per track    */
    uint8_t max_track;        /* Nb of tracks           */
89
    uint16_t bps;             /* Bytes per sector       */
90
91
92
    uint8_t ro;               /* Is read-only           */
} fdrive_t;

93
static void fd_init (fdrive_t *drv, BlockDriverState *bs)
94
95
{
    /* Drive */
96
97
98
99
100
    drv->bs = bs;
    if (bs)
        drv->drive = FDRIVE_DRV_144;
    else
        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
    drv->drflags &= ~FDRIVE_REVALIDATE;
239
    if (drv->bs != NULL && bdrv_is_inserted(drv->bs)) {
240
	ro = bdrv_is_read_only(drv->bs);
241
	bdrv_get_geometry_hint(drv->bs, &nb_heads, &max_track, &last_sect);
242
	if (nb_heads != 0 && max_track != 0 && last_sect != 0) {
243
244
	    FLOPPY_DPRINTF("User defined disk (%d %d %d)",
                           nb_heads - 1, max_track, last_sect);
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
275
	} 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;
276
277
	    FLOPPY_DPRINTF("%s floppy disk (%d h %d t %d s) %s\n", parse->str,
                           nb_heads, max_track, last_sect, ro ? "ro" : "rw");
278
	}
279
280
281
282
283
284
285
286
	    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;
287
    } else {
288
	FLOPPY_DPRINTF("No disk in drive\n");
289
290
291
        drv->last_sect = 0;
	drv->max_track = 0;
	drv->flags &= ~FDISK_DBL_SIDES;
292
    }
293
    drv->drflags |= FDRIVE_REVALIDATE;
294
295
}

296
297
298
/* Motor control */
static void fd_start (fdrive_t *drv)
{
299
    drv->drflags |= FDRIVE_MOTOR_ON;
300
301
302
303
}

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

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

/********************************************************/
/* Intel 82078 floppy disk controler emulation          */

317
318
static void fdctrl_reset (fdctrl_t *fdctrl, int do_irq);
static void fdctrl_reset_fifo (fdctrl_t *fdctrl);
319
static int fdctrl_transfer_handler (void *opaque, target_ulong addr, int size);
320
static void fdctrl_raise_irq (fdctrl_t *fdctrl, uint8_t status);
321
static void fdctrl_result_timer(void *opaque);
322
323
324
325
326
327
328
329
330
331
332

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

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

#define FD_STATE(state) ((state) & FD_STATE_STATE)
361
362
#define FD_SET_STATE(state, new_state) \
do { (state) = ((state) & ~FD_STATE_STATE) | (new_state); } while (0)
363
364
#define FD_MULTI_TRACK(state) ((state) & FD_STATE_MULTI)
#define FD_DID_SEEK(state) ((state) & FD_STATE_SEEK)
365
#define FD_FORMAT_CMD(state) ((state) & FD_STATE_FORMAT)
366

367
368
struct fdctrl_t {
    fdctrl_t *fdctrl;
369
370
371
372
373
    /* Controler's identification */
    uint8_t version;
    /* HW */
    int irq_lvl;
    int dma_chann;
374
    uint32_t io_base;
375
    /* Controler state */
376
    QEMUTimer *result_timer;
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
    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;
    /* 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
408
    switch (reg & 0x07) {
    case 0x01:
409
	retval = fdctrl_read_statusB(fdctrl);
410
411
	break;
    case 0x02:
412
	retval = fdctrl_read_dor(fdctrl);
413
414
	break;
    case 0x03:
415
        retval = fdctrl_read_tape(fdctrl);
416
417
	break;
    case 0x04:
418
        retval = fdctrl_read_main_status(fdctrl);
419
420
	break;
    case 0x05:
421
        retval = fdctrl_read_data(fdctrl);
422
423
	break;
    case 0x07:
424
        retval = fdctrl_read_dir(fdctrl);
425
426
	break;
    default:
427
	retval = (uint32_t)(-1);
428
429
	break;
    }
430
    FLOPPY_DPRINTF("read reg%d: 0x%02x\n", reg & 7, retval);
431
432
433
434
435
436
437
438

    return retval;
}

static void fdctrl_write (void *opaque, uint32_t reg, uint32_t value)
{
    fdctrl_t *fdctrl = opaque;

439
440
    FLOPPY_DPRINTF("write reg%d: 0x%02x\n", reg & 7, value);

441
442
    switch (reg & 0x07) {
    case 0x02:
443
	fdctrl_write_dor(fdctrl, value);
444
445
	break;
    case 0x03:
446
        fdctrl_write_tape(fdctrl, value);
447
448
	break;
    case 0x04:
449
        fdctrl_write_rate(fdctrl, value);
450
451
	break;
    case 0x05:
452
        fdctrl_write_data(fdctrl, value);
453
454
455
456
	break;
    default:
	break;
    }
457
458
459
460
461
}

static void fd_change_cb (void *opaque)
{
    fdrive_t *drv = opaque;
462

463
464
465
466
467
468
469
470
    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
}
471

472
473
474
fdctrl_t *fdctrl_init (int irq_lvl, int dma_chann, int mem_mapped, 
                       uint32_t io_base,
                       BlockDriverState **fds)
475
{
476
    fdctrl_t *fdctrl;
477
478
479
480
//    int io_mem;
    int i;

    FLOPPY_DPRINTF("init controler\n");
481
482
483
    fdctrl = qemu_mallocz(sizeof(fdctrl_t));
    if (!fdctrl)
        return NULL;
484
485
486
    fdctrl->result_timer = qemu_new_timer(vm_clock, 
                                          fdctrl_result_timer, fdctrl);

487
488
489
490
    fdctrl->version = 0x90; /* Intel 82078 controler */
    fdctrl->irq_lvl = irq_lvl;
    fdctrl->dma_chann = dma_chann;
    fdctrl->io_base = io_base;
491
    fdctrl->config = 0x60; /* Implicit seek, polling & FIFO enabled */
492
493
494
    if (fdctrl->dma_chann != -1) {
        fdctrl->dma_en = 1;
        DMA_register_channel(dma_chann, &fdctrl_transfer_handler, fdctrl);
495
    } else {
496
        fdctrl->dma_en = 0;
497
    }
498
499
500
501
502
503
    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]);
        }
504
    }
505
506
    fdctrl_reset(fdctrl, 0);
    fdctrl->state = FD_CTRL_ACTIVE;
507
508
509
510
511
512
513
    if (mem_mapped) {
        FLOPPY_ERROR("memory mapped floppy not supported by now !\n");
#if 0
        io_mem = cpu_register_io_memory(0, fdctrl_mem_read, fdctrl_mem_write);
        cpu_register_physical_memory(base, 0x08, io_mem);
#endif
    } else {
514
515
516
517
        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);
518
    }
519
    for (i = 0; i < 2; i++) {
520
        fd_revalidate(&fdctrl->drives[i]);
521
    }
522

523
    return fdctrl;
524
}
525

526
527
/* XXX: may change if moved to bdrv */
int fdctrl_get_drive_type(fdctrl_t *fdctrl, int drive_num)
528
{
529
    return fdctrl->drives[drive_num].drive;
530
531
532
}

/* Change IRQ state */
533
static void fdctrl_reset_irq (fdctrl_t *fdctrl)
534
{
535
536
537
    FLOPPY_DPRINTF("Reset interrupt\n");
    pic_set_irq(fdctrl->irq_lvl, 0);
    fdctrl->state &= ~FD_CTRL_INTR;
538
539
}

540
static void fdctrl_raise_irq (fdctrl_t *fdctrl, uint8_t status)
541
{
542
543
544
    if (~(fdctrl->state & FD_CTRL_INTR)) {
        pic_set_irq(fdctrl->irq_lvl, 1);
        fdctrl->state |= FD_CTRL_INTR;
545
546
    }
    FLOPPY_DPRINTF("Set interrupt status to 0x%02x\n", status);
547
    fdctrl->int_status = status;
548
549
550
}

/* Reset controler */
551
static void fdctrl_reset (fdctrl_t *fdctrl, int do_irq)
552
553
554
555
{
    int i;

    FLOPPY_DPRINTF("reset controler\n");
556
    fdctrl_reset_irq(fdctrl);
557
    /* Initialise controler */
558
    fdctrl->cur_drv = 0;
559
    /* FIFO state */
560
561
562
563
    fdctrl->data_pos = 0;
    fdctrl->data_len = 0;
    fdctrl->data_state = FD_STATE_CMD;
    fdctrl->data_dir = FD_DIR_WRITE;
564
    for (i = 0; i < MAX_FD; i++)
565
566
        fd_reset(&fdctrl->drives[i]);
    fdctrl_reset_fifo(fdctrl);
567
    if (do_irq)
568
        fdctrl_raise_irq(fdctrl, 0xc0);
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
}

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);
584
585
586
}

/* Status B register : 0x01 (read-only) */
587
static uint32_t fdctrl_read_statusB (fdctrl_t *fdctrl)
588
589
590
591
592
593
{
    FLOPPY_DPRINTF("status register: 0x00\n");
    return 0;
}

/* Digital output register : 0x02 */
594
static uint32_t fdctrl_read_dor (fdctrl_t *fdctrl)
595
596
597
598
{
    uint32_t retval = 0;

    /* Drive motors state indicators */
599
600
601
602
    if (drv0(fdctrl)->drflags & FDRIVE_MOTOR_ON)
	retval |= 1 << 5;
    if (drv1(fdctrl)->drflags & FDRIVE_MOTOR_ON)
	retval |= 1 << 4;
603
    /* DMA enable */
604
    retval |= fdctrl->dma_en << 3;
605
    /* Reset indicator */
606
    retval |= (fdctrl->state & FD_CTRL_RESET) == 0 ? 0x04 : 0;
607
    /* Selected drive */
608
    retval |= fdctrl->cur_drv;
609
610
611
612
613
    FLOPPY_DPRINTF("digital output register: 0x%02x\n", retval);

    return retval;
}

614
static void fdctrl_write_dor (fdctrl_t *fdctrl, uint32_t value)
615
616
{
    /* Reset mode */
617
    if (fdctrl->state & FD_CTRL_RESET) {
618
619
620
621
622
623
624
625
        if (!(value & 0x04)) {
            FLOPPY_DPRINTF("Floppy controler in RESET state !\n");
            return;
        }
    }
    FLOPPY_DPRINTF("digital output register set to 0x%02x\n", value);
    /* Drive motors state indicators */
    if (value & 0x20)
626
        fd_start(drv1(fdctrl));
627
    else
628
        fd_stop(drv1(fdctrl));
629
    if (value & 0x10)
630
        fd_start(drv0(fdctrl));
631
    else
632
        fd_stop(drv0(fdctrl));
633
634
    /* DMA enable */
#if 0
635
636
    if (fdctrl->dma_chann != -1)
        fdctrl->dma_en = 1 - ((value >> 3) & 1);
637
638
639
#endif
    /* Reset */
    if (!(value & 0x04)) {
640
        if (!(fdctrl->state & FD_CTRL_RESET)) {
641
            FLOPPY_DPRINTF("controler enter RESET state\n");
642
            fdctrl->state |= FD_CTRL_RESET;
643
644
        }
    } else {
645
        if (fdctrl->state & FD_CTRL_RESET) {
646
            FLOPPY_DPRINTF("controler out of RESET state\n");
647
            fdctrl_reset(fdctrl, 1);
648
            fdctrl->state &= ~(FD_CTRL_RESET | FD_CTRL_SLEEP);
649
650
651
        }
    }
    /* Selected drive */
652
    fdctrl->cur_drv = value & 1;
653
654
655
}

/* Tape drive register : 0x03 */
656
static uint32_t fdctrl_read_tape (fdctrl_t *fdctrl)
657
658
659
660
{
    uint32_t retval = 0;

    /* Disk boot selection indicator */
661
    retval |= fdctrl->bootsel << 2;
662
663
664
665
666
667
    /* Tape indicators: never allowed */
    FLOPPY_DPRINTF("tape drive register: 0x%02x\n", retval);

    return retval;
}

668
static void fdctrl_write_tape (fdctrl_t *fdctrl, uint32_t value)
669
670
{
    /* Reset mode */
671
    if (fdctrl->state & FD_CTRL_RESET) {
672
673
674
675
676
        FLOPPY_DPRINTF("Floppy controler in RESET state !\n");
        return;
    }
    FLOPPY_DPRINTF("tape drive register set to 0x%02x\n", value);
    /* Disk boot selection indicator */
677
    fdctrl->bootsel = (value >> 2) & 1;
678
679
680
681
    /* Tape indicators: never allow */
}

/* Main status register : 0x04 (read) */
682
static uint32_t fdctrl_read_main_status (fdctrl_t *fdctrl)
683
684
685
{
    uint32_t retval = 0;

686
687
    fdctrl->state &= ~(FD_CTRL_SLEEP | FD_CTRL_RESET);
    if (!(fdctrl->state & FD_CTRL_BUSY)) {
688
689
690
        /* Data transfer allowed */
        retval |= 0x80;
        /* Data transfer direction indicator */
691
        if (fdctrl->data_dir == FD_DIR_READ)
692
693
694
695
            retval |= 0x40;
    }
    /* Should handle 0x20 for SPECIFY command */
    /* Command busy indicator */
696
697
    if (FD_STATE(fdctrl->data_state) == FD_STATE_DATA ||
        FD_STATE(fdctrl->data_state) == FD_STATE_STATUS)
698
699
700
701
702
703
704
        retval |= 0x10;
    FLOPPY_DPRINTF("main status register: 0x%02x\n", retval);

    return retval;
}

/* Data select rate register : 0x04 (write) */
705
static void fdctrl_write_rate (fdctrl_t *fdctrl, uint32_t value)
706
707
{
    /* Reset mode */
708
    if (fdctrl->state & FD_CTRL_RESET) {
709
710
711
712
713
714
            FLOPPY_DPRINTF("Floppy controler in RESET state !\n");
            return;
        }
    FLOPPY_DPRINTF("select rate register set to 0x%02x\n", value);
    /* Reset: autoclear */
    if (value & 0x80) {
715
716
717
        fdctrl->state |= FD_CTRL_RESET;
        fdctrl_reset(fdctrl, 1);
        fdctrl->state &= ~FD_CTRL_RESET;
718
719
    }
    if (value & 0x40) {
720
721
        fdctrl->state |= FD_CTRL_SLEEP;
        fdctrl_reset(fdctrl, 1);
722
723
724
725
726
    }
//        fdctrl.precomp = (value >> 2) & 0x07;
}

/* Digital input register : 0x07 (read-only) */
727
static uint32_t fdctrl_read_dir (fdctrl_t *fdctrl)
728
729
730
{
    uint32_t retval = 0;

731
732
    if (drv0(fdctrl)->drflags & FDRIVE_REVALIDATE ||
	drv1(fdctrl)->drflags & FDRIVE_REVALIDATE)
733
734
        retval |= 0x80;
    if (retval != 0)
735
736
737
        FLOPPY_DPRINTF("Floppy digital input register: 0x%02x\n", retval);
    drv0(fdctrl)->drflags &= ~FDRIVE_REVALIDATE;
    drv1(fdctrl)->drflags &= ~FDRIVE_REVALIDATE;
738
739
740
741
742

    return retval;
}

/* FIFO state control */
743
static void fdctrl_reset_fifo (fdctrl_t *fdctrl)
744
{
745
746
747
    fdctrl->data_dir = FD_DIR_WRITE;
    fdctrl->data_pos = 0;
    FD_SET_STATE(fdctrl->data_state, FD_STATE_CMD);
748
749
750
}

/* Set FIFO status for the host to read */
751
static void fdctrl_set_fifo (fdctrl_t *fdctrl, int fifo_len, int do_irq)
752
{
753
754
755
756
    fdctrl->data_dir = FD_DIR_READ;
    fdctrl->data_len = fifo_len;
    fdctrl->data_pos = 0;
    FD_SET_STATE(fdctrl->data_state, FD_STATE_STATUS);
757
    if (do_irq)
758
        fdctrl_raise_irq(fdctrl, 0x00);
759
760
761
}

/* Set an error: unimplemented/unknown command */
762
static void fdctrl_unimplemented (fdctrl_t *fdctrl)
763
764
{
#if 0
765
766
767
768
769
770
771
    fdrive_t *cur_drv;

    cur_drv = get_cur_drv(fdctrl);
    fdctrl->fifo[0] = 0x60 | (cur_drv->head << 1) | fdctrl->cur_drv;
    fdctrl->fifo[1] = 0x00;
    fdctrl->fifo[2] = 0x00;
    fdctrl_set_fifo(fdctrl, 3, 1);
772
#else
773
774
775
    //    fdctrl_reset_fifo(fdctrl);
    fdctrl->fifo[0] = 0x80;
    fdctrl_set_fifo(fdctrl, 1, 0);
776
777
778
779
#endif
}

/* Callback for transfer end (stop or abort) */
780
781
static void fdctrl_stop_transfer (fdctrl_t *fdctrl, uint8_t status0,
				  uint8_t status1, uint8_t status2)
782
{
783
    fdrive_t *cur_drv;
784

785
    cur_drv = get_cur_drv(fdctrl);
786
787
    FLOPPY_DPRINTF("transfer status: %02x %02x %02x (%02x)\n",
                   status0, status1, status2,
788
789
790
791
792
793
794
795
796
                   status0 | (cur_drv->head << 1) | fdctrl->cur_drv);
    fdctrl->fifo[0] = status0 | (cur_drv->head << 1) | fdctrl->cur_drv;
    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;
797
    if (fdctrl->state & FD_CTRL_BUSY) {
798
        DMA_release_DREQ(fdctrl->dma_chann);
799
800
        fdctrl->state &= ~FD_CTRL_BUSY;
    }
801
    fdctrl_set_fifo(fdctrl, 7, 1);
802
803
804
}

/* Prepare a data transfer (either DMA or FIFO) */
805
static void fdctrl_start_transfer (fdctrl_t *fdctrl, int direction)
806
{
807
    fdrive_t *cur_drv;
808
809
810
    uint8_t kh, kt, ks;
    int did_seek;

811
812
813
814
815
    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];
816
    FLOPPY_DPRINTF("Start tranfert at %d %d %02x %02x (%d)\n",
817
                   fdctrl->cur_drv, kh, kt, ks,
818
819
                   _fd_sector(kh, kt, ks, cur_drv->last_sect));
    did_seek = 0;
820
    switch (fd_seek(cur_drv, kh, kt, ks, fdctrl->config & 0x40)) {
821
822
    case 2:
        /* sect too big */
823
824
825
826
        fdctrl_stop_transfer(fdctrl, 0x40, 0x00, 0x00);
        fdctrl->fifo[3] = kt;
        fdctrl->fifo[4] = kh;
        fdctrl->fifo[5] = ks;
827
828
829
        return;
    case 3:
        /* track too big */
830
831
832
833
        fdctrl_stop_transfer(fdctrl, 0x40, 0x80, 0x00);
        fdctrl->fifo[3] = kt;
        fdctrl->fifo[4] = kh;
        fdctrl->fifo[5] = ks;
834
835
836
        return;
    case 4:
        /* No seek enabled */
837
838
839
840
        fdctrl_stop_transfer(fdctrl, 0x40, 0x00, 0x00);
        fdctrl->fifo[3] = kt;
        fdctrl->fifo[4] = kh;
        fdctrl->fifo[5] = ks;
841
842
843
844
845
846
847
848
        return;
    case 1:
        did_seek = 1;
        break;
    default:
        break;
    }
    /* Set the FIFO state */
849
850
851
852
853
854
855
    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;
856
    if (did_seek)
857
858
859
860
861
862
863
864
865
866
867
868
869
870
        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;
    }
    if (fdctrl->dma_en) {
871
872
        int dma_mode;
        /* DMA transfer are enabled. Check if DMA channel is well programmed */
873
        dma_mode = DMA_get_channel_mode(fdctrl->dma_chann);
874
        dma_mode = (dma_mode >> 2) & 3;
875
876
877
878
        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);
879
880
881
882
883
        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 */
884
            fdctrl->state |= FD_CTRL_BUSY;
885
886
887
            /* Now, we just have to wait for the DMA controler to
             * recall us...
             */
888
889
            DMA_hold_DREQ(fdctrl->dma_chann);
            DMA_schedule(fdctrl->dma_chann);
890
            return;
891
892
        } else {
	    FLOPPY_ERROR("dma_mode=%d direction=%d\n", dma_mode, direction);
893
894
895
896
        }
    }
    FLOPPY_DPRINTF("start non-DMA transfer\n");
    /* IO based transfer: calculate len */
897
    fdctrl_raise_irq(fdctrl, 0x00);
898
899
900
901
902

    return;
}

/* Prepare a transfer of deleted data */
903
static void fdctrl_start_transfer_del (fdctrl_t *fdctrl, int direction)
904
905
906
907
{
    /* We don't handle deleted data,
     * so we don't return *ANYTHING*
     */
908
    fdctrl_stop_transfer(fdctrl, 0x60, 0x00, 0x00);
909
910
911
}

/* handlers for DMA transfers */
912
static int fdctrl_transfer_handler (void *opaque, target_ulong addr, int size)
913
{
914
915
916
    fdctrl_t *fdctrl;
    fdrive_t *cur_drv;
    int len, start_pos, rel_pos;
917
918
    uint8_t status0 = 0x00, status1 = 0x00, status2 = 0x00;

919
920
    fdctrl = opaque;
    if (!(fdctrl->state & FD_CTRL_BUSY)) {
921
922
923
        FLOPPY_DPRINTF("Not in DMA transfer mode !\n");
        return 0;
    }
924
925
926
    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)
927
        status2 = 0x04;
928
929
    if (size > fdctrl->data_len)
	size = fdctrl->data_len;
930
            if (cur_drv->bs == NULL) {
931
932
933
934
935
	if (fdctrl->data_dir == FD_DIR_WRITE)
	    fdctrl_stop_transfer(fdctrl, 0x60, 0x00, 0x00);
	else
	    fdctrl_stop_transfer(fdctrl, 0x40, 0x00, 0x00);
	len = 0;
936
937
                goto transfer_error;
            }
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
    rel_pos = fdctrl->data_pos % FD_SECTOR_LEN;
    for (start_pos = fdctrl->data_pos; fdctrl->data_pos < size;) {
        len = size - fdctrl->data_pos;
        if (len + rel_pos > FD_SECTOR_LEN)
            len = FD_SECTOR_LEN - rel_pos;
        FLOPPY_DPRINTF("copy %d bytes (%d %d %d) %d pos %d %02x %02x "
                       "(%d-0x%08x 0x%08x)\n", len, size, fdctrl->data_pos,
                       fdctrl->data_len, fdctrl->cur_drv, cur_drv->head,
                       cur_drv->track, cur_drv->sect, fd_sector(cur_drv),
                       fd_sector(cur_drv) * 512, addr);
        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) {
953
954
955
                FLOPPY_DPRINTF("Floppy: error getting sector %d\n",
                               fd_sector(cur_drv));
                /* Sure, image size is too small... */
956
                memset(fdctrl->fifo, 0, FD_SECTOR_LEN);
957
958
            }
                }
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
	switch (fdctrl->data_dir) {
	case FD_DIR_READ:
	    /* READ commands */
	    cpu_physical_memory_write(addr + fdctrl->data_pos,
				      fdctrl->fifo + rel_pos, len);
	    break;
	case FD_DIR_WRITE:
            /* WRITE commands */
            cpu_physical_memory_read(addr + fdctrl->data_pos,
				     fdctrl->fifo + rel_pos, len);
            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;
974
                }
975
976
977
978
979
980
981
982
983
	    break;
	default:
	    /* SCAN commands */
            {
		uint8_t tmpbuf[FD_SECTOR_LEN];
                int ret;
                cpu_physical_memory_read(addr + fdctrl->data_pos,
                                         tmpbuf, len);
                ret = memcmp(tmpbuf, fdctrl->fifo + rel_pos, len);
984
985
986
987
                if (ret == 0) {
                    status2 = 0x08;
                    goto end_transfer;
                }
988
989
                if ((ret < 0 && fdctrl->data_dir == FD_DIR_SCANL) ||
                    (ret > 0 && fdctrl->data_dir == FD_DIR_SCANH)) {
990
991
992
993
                    status2 = 0x00;
                    goto end_transfer;
                }
            }
994
	    break;
995
        }
996
997
998
	fdctrl->data_pos += len;
	rel_pos = fdctrl->data_pos % FD_SECTOR_LEN;
        if (rel_pos == 0) {
999
            /* Seek to next sector */
1000
1001
1002
1003
1004
1005
1006
1007
1008
1009
	    cur_drv->sect++;
	    FLOPPY_DPRINTF("seek to next sector (%d %02x %02x => %d) (%d)\n",
			   cur_drv->head, cur_drv->track, cur_drv->sect,
			   fd_sector(cur_drv),
			   fdctrl->data_pos - size);
            if (cur_drv->sect > cur_drv->last_sect) {
		cur_drv->sect = 1;
		if (FD_MULTI_TRACK(fdctrl->data_state)) {
		    if (cur_drv->head == 0 &&
			(cur_drv->flags & FDISK_DBL_SIDES) != 0) {	
1010
1011
1012
                    cur_drv->head = 1;
                } else {
                    cur_drv->head = 0;
1013
1014
1015
			cur_drv->track++;
			if ((cur_drv->flags & FDISK_DBL_SIDES) == 0)
			    break;
1016
1017
                }
            } else {
1018
1019
1020
1021
1022
1023
		    cur_drv->track++;
		    break;
		}
		FLOPPY_DPRINTF("seek to next track (%d %02x %02x => %d)\n",
			       cur_drv->head, cur_drv->track,
			       cur_drv->sect, fd_sector(cur_drv));
1024
1025
1026
1027
            }
        }
    }
end_transfer:
1028
1029
1030
1031
1032
1033
    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)
1034
        status2 = 0x08;
1035
    if (FD_DID_SEEK(fdctrl->data_state))
1036
        status0 |= 0x20;
1037
1038
1039
    fdctrl->data_len -= len;
    //    if (fdctrl->data_len == 0)
	fdctrl_stop_transfer(fdctrl, status0, status1, status2);
1040
1041
transfer_error:

1042
    return len;
1043
1044
1045
}

/* Data register : 0x05 */
1046
static uint32_t fdctrl_read_data (fdctrl_t *fdctrl)
1047
{
1048
    fdrive_t *cur_drv;
1049
1050
1051
    uint32_t retval = 0;
    int pos, len;

1052
1053
1054
    cur_drv = get_cur_drv(fdctrl);
    fdctrl->state &= ~FD_CTRL_SLEEP;
    if (FD_STATE(fdctrl->data_state) == FD_STATE_CMD) {
1055
1056
1057
        FLOPPY_ERROR("can't read data in CMD state\n");
        return 0;
    }
1058
1059
    pos = fdctrl->data_pos;
    if (FD_STATE(fdctrl->data_state) == FD_STATE_DATA) {
1060
1061
        pos %= FD_SECTOR_LEN;
        if (pos == 0) {
1062
            len = fdctrl->data_len - fdctrl->data_pos;
1063
1064
1065
            if (len > FD_SECTOR_LEN)
                len = FD_SECTOR_LEN;
            bdrv_read(cur_drv->bs, fd_sector(cur_drv),
1066
                      fdctrl->fifo, len);
1067
1068
        }
    }
1069
1070
1071
    retval = fdctrl->fifo[pos];
    if (++fdctrl->data_pos == fdctrl->data_len) {
        fdctrl->data_pos = 0;
1072
1073
1074
        /* Switch from transfert mode to status mode
         * then from status mode to command mode
         */
1075
        if (FD_STATE(fdctrl->data_state) == FD_STATE_DATA) {
1076
            fdctrl_stop_transfer(fdctrl, 0x20, 0x00, 0x00);
1077
        } else {
1078
            fdctrl_reset_fifo(fdctrl);
1079
1080
            fdctrl_reset_irq(fdctrl);
        }
1081
1082
1083
1084
1085
1086
    }
    FLOPPY_DPRINTF("data register: 0x%02x\n", retval);

    return retval;
}

1087
static void fdctrl_format_sector (fdctrl_t *fdctrl)
1088
{
1089
1090
1091
    fdrive_t *cur_drv;
    uint8_t kh, kt, ks;
    int did_seek;
1092

1093
1094
1095
1096
1097
1098
1099
1100
1101
1102
1103
1104
1105
1106
1107
1108
1109
1110
1111
1112
1113
1114
1115
1116
1117
1118
1119
1120
1121
1122
1123
1124
1125
1126
1127
1128
1129
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
    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);
1157
    /* Reset mode */
1158
    if (fdctrl->state & FD_CTRL_RESET) {
1159
1160
1161
        FLOPPY_DPRINTF("Floppy controler in RESET state !\n");
        return;
    }
1162
1163
    fdctrl->state &= ~FD_CTRL_SLEEP;
    if (FD_STATE(fdctrl->data_state) == FD_STATE_STATUS) {
1164
1165
1166
1167
        FLOPPY_ERROR("can't write data in status mode\n");
        return;
    }
    /* Is it write command time ? */
1168
    if (FD_STATE(fdctrl->data_state) == FD_STATE_DATA) {
1169
        /* FIFO data write */
1170
1171
1172
        fdctrl->fifo[fdctrl->data_pos++] = value;
        if (fdctrl->data_pos % FD_SECTOR_LEN == (FD_SECTOR_LEN - 1) ||
            fdctrl->data_pos == fdctrl->data_len) {
1173
            bdrv_write(cur_drv->bs, fd_sector(cur_drv),
1174
                       fdctrl->fifo, FD_SECTOR_LEN);
1175
1176
1177
1178
        }
        /* Switch from transfert mode to status mode
         * then from status mode to command mode
         */
1179
1180
        if (FD_STATE(fdctrl->data_state) == FD_STATE_DATA)
            fdctrl_stop_transfer(fdctrl, 0x20, 0x00, 0x00);
1181
1182
        return;
    }
1183
    if (fdctrl->data_pos == 0) {
1184
1185
1186
1187
1188
1189
        /* Command */
        switch (value & 0x5F) {
        case 0x46:
            /* READ variants */
            FLOPPY_DPRINTF("READ command\n");
            /* 8 parameters cmd */
1190
            fdctrl->data_len = 9;
1191
1192
1193
1194
1195
            goto enqueue;
        case 0x4C:
            /* READ_DELETED variants */
            FLOPPY_DPRINTF("READ_DELETED command\n");
            /* 8 parameters cmd */
1196
            fdctrl->data_len = 9;
1197
1198
1199
1200
1201
            goto enqueue;
        case 0x50:
            /* SCAN_EQUAL variants */
            FLOPPY_DPRINTF("SCAN_EQUAL command\n");
            /* 8 parameters cmd */
1202
            fdctrl->data_len = 9;
bellard's avatar