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

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

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

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

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

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

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

typedef enum fdrive_type_t {
    FDRIVE_DRV_144  = 0x00,   /* 1.44 MB 3"5 drive      */
    FDRIVE_DRV_288  = 0x01,   /* 2.88 MB 3"5 drive      */
    FDRIVE_DRV_120  = 0x02,   /* 1.2  MB 5"25 drive     */
    FDRIVE_DRV_NONE = 0x03,   /* No drive connected     */
} fdrive_type_t;

67
68
69
70
71
72
73
74
typedef enum fdrive_flags_t {
    FDRIVE_MOTOR_ON   = 0x01, /* motor on/off           */
} fdrive_flags_t;

typedef enum fdisk_flags_t {
    FDISK_DBL_SIDES  = 0x01,
} fdisk_flags_t;

75
76
77
78
typedef struct fdrive_t {
    BlockDriverState *bs;
    /* Drive status */
    fdrive_type_t drive;
79
    fdrive_flags_t drflags;
80
81
82
83
84
85
86
87
88
    uint8_t perpendicular;    /* 2.88 MB access mode    */
    /* Position */
    uint8_t head;
    uint8_t track;
    uint8_t sect;
    /* Last operation status */
    uint8_t dir;              /* Direction              */
    uint8_t rw;               /* Read/write             */
    /* Media */
89
    fdisk_flags_t flags;
90
91
    uint8_t last_sect;        /* Nb sector per track    */
    uint8_t max_track;        /* Nb of tracks           */
92
    uint16_t bps;             /* Bytes per sector       */
93
94
95
    uint8_t ro;               /* Is read-only           */
} fdrive_t;

96
static void fd_init (fdrive_t *drv, BlockDriverState *bs)
97
98
{
    /* Drive */
99
    drv->bs = bs;
bellard's avatar
bellard committed
100
    drv->drive = FDRIVE_DRV_NONE;
101
    drv->drflags = 0;
102
103
    drv->perpendicular = 0;
    /* Disk */
104
    drv->last_sect = 0;
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
    drv->max_track = 0;
}

static int _fd_sector (uint8_t head, uint8_t track,
                        uint8_t sect, uint8_t last_sect)
{
    return (((track * 2) + head) * last_sect) + sect - 1;
}

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

static int fd_seek (fdrive_t *drv, uint8_t head, uint8_t track, uint8_t sect,
                    int enable_seek)
{
    uint32_t sector;
124
125
126
127
    int ret;

    if (track > drv->max_track ||
	(head != 0 && (drv->flags & FDISK_DBL_SIDES) == 0)) {
128
129
130
131
        FLOPPY_DPRINTF("try to read %d %02x %02x (max=%d %d %02x %02x)\n",
                       head, track, sect, 1,
                       (drv->flags & FDISK_DBL_SIDES) == 0 ? 0 : 1,
                       drv->max_track, drv->last_sect);
132
133
134
        return 2;
    }
    if (sect > drv->last_sect) {
135
136
137
138
        FLOPPY_DPRINTF("try to read %d %02x %02x (max=%d %d %02x %02x)\n",
                       head, track, sect, 1,
                       (drv->flags & FDISK_DBL_SIDES) == 0 ? 0 : 1,
                       drv->max_track, drv->last_sect);
139
140
141
        return 3;
    }
    sector = _fd_sector(head, track, sect, drv->last_sect);
142
    ret = 0;
143
144
145
146
147
148
149
150
151
    if (sector != fd_sector(drv)) {
#if 0
        if (!enable_seek) {
            FLOPPY_ERROR("no implicit seek %d %02x %02x (max=%d %02x %02x)\n",
                         head, track, sect, 1, drv->max_track, drv->last_sect);
            return 4;
        }
#endif
        drv->head = head;
152
153
	if (drv->track != track)
	    ret = 1;
154
155
156
157
        drv->track = track;
        drv->sect = sect;
    }

158
    return ret;
159
160
161
162
163
164
165
166
167
168
169
170
171
}

/* Set drive back to track 0 */
static void fd_recalibrate (fdrive_t *drv)
{
    FLOPPY_DPRINTF("recalibrate\n");
    drv->head = 0;
    drv->track = 0;
    drv->sect = 1;
    drv->dir = 1;
    drv->rw = 0;
}

172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
/* Recognize floppy formats */
typedef struct fd_format_t {
    fdrive_type_t drive;
    fdisk_type_t  disk;
    uint8_t last_sect;
    uint8_t max_track;
    uint8_t max_head;
    const unsigned char *str;
} fd_format_t;

static fd_format_t fd_formats[] = {
    /* First entry is default format */
    /* 1.44 MB 3"1/2 floppy disks */
    { FDRIVE_DRV_144, FDRIVE_DISK_144, 18, 80, 1, "1.44 MB 3\"1/2", },
    { FDRIVE_DRV_144, FDRIVE_DISK_144, 20, 80, 1,  "1.6 MB 3\"1/2", },
    { FDRIVE_DRV_144, FDRIVE_DISK_144, 21, 80, 1, "1.68 MB 3\"1/2", },
    { FDRIVE_DRV_144, FDRIVE_DISK_144, 21, 82, 1, "1.72 MB 3\"1/2", },
    { FDRIVE_DRV_144, FDRIVE_DISK_144, 21, 83, 1, "1.74 MB 3\"1/2", },
    { FDRIVE_DRV_144, FDRIVE_DISK_144, 22, 80, 1, "1.76 MB 3\"1/2", },
    { FDRIVE_DRV_144, FDRIVE_DISK_144, 23, 80, 1, "1.84 MB 3\"1/2", },
    { FDRIVE_DRV_144, FDRIVE_DISK_144, 24, 80, 1, "1.92 MB 3\"1/2", },
    /* 2.88 MB 3"1/2 floppy disks */
    { FDRIVE_DRV_288, FDRIVE_DISK_288, 36, 80, 1, "2.88 MB 3\"1/2", },
    { FDRIVE_DRV_288, FDRIVE_DISK_288, 39, 80, 1, "3.12 MB 3\"1/2", },
    { FDRIVE_DRV_288, FDRIVE_DISK_288, 40, 80, 1,  "3.2 MB 3\"1/2", },
    { FDRIVE_DRV_288, FDRIVE_DISK_288, 44, 80, 1, "3.52 MB 3\"1/2", },
    { FDRIVE_DRV_288, FDRIVE_DISK_288, 48, 80, 1, "3.84 MB 3\"1/2", },
    /* 720 kB 3"1/2 floppy disks */
    { FDRIVE_DRV_144, FDRIVE_DISK_720,  9, 80, 1,  "720 kB 3\"1/2", },
    { FDRIVE_DRV_144, FDRIVE_DISK_720, 10, 80, 1,  "800 kB 3\"1/2", },
    { FDRIVE_DRV_144, FDRIVE_DISK_720, 10, 82, 1,  "820 kB 3\"1/2", },
    { FDRIVE_DRV_144, FDRIVE_DISK_720, 10, 83, 1,  "830 kB 3\"1/2", },
    { FDRIVE_DRV_144, FDRIVE_DISK_720, 13, 80, 1, "1.04 MB 3\"1/2", },
    { FDRIVE_DRV_144, FDRIVE_DISK_720, 14, 80, 1, "1.12 MB 3\"1/2", },
    /* 1.2 MB 5"1/4 floppy disks */
    { FDRIVE_DRV_120, FDRIVE_DISK_288, 15, 80, 1,  "1.2 kB 5\"1/4", },
    { FDRIVE_DRV_120, FDRIVE_DISK_288, 18, 80, 1, "1.44 MB 5\"1/4", },
    { FDRIVE_DRV_120, FDRIVE_DISK_288, 18, 82, 1, "1.48 MB 5\"1/4", },
    { FDRIVE_DRV_120, FDRIVE_DISK_288, 18, 83, 1, "1.49 MB 5\"1/4", },
    { FDRIVE_DRV_120, FDRIVE_DISK_288, 20, 80, 1,  "1.6 MB 5\"1/4", },
    /* 720 kB 5"1/4 floppy disks */
    { FDRIVE_DRV_120, FDRIVE_DISK_288,  9, 80, 1,  "720 kB 5\"1/4", },
    { FDRIVE_DRV_120, FDRIVE_DISK_288, 11, 80, 1,  "880 kB 5\"1/4", },
    /* 360 kB 5"1/4 floppy disks */
    { FDRIVE_DRV_120, FDRIVE_DISK_288,  9, 40, 1,  "360 kB 5\"1/4", },
    { FDRIVE_DRV_120, FDRIVE_DISK_288,  9, 40, 0,  "180 kB 5\"1/4", },
    { FDRIVE_DRV_120, FDRIVE_DISK_288, 10, 41, 1,  "410 kB 5\"1/4", },
    { FDRIVE_DRV_120, FDRIVE_DISK_288, 10, 42, 1,  "420 kB 5\"1/4", },
    /* 320 kB 5"1/4 floppy disks */ 
    { FDRIVE_DRV_120, FDRIVE_DISK_288,  8, 40, 1,  "320 kB 5\"1/4", },
    { FDRIVE_DRV_120, FDRIVE_DISK_288,  8, 40, 0,  "160 kB 5\"1/4", },
    /* 360 kB must match 5"1/4 better than 3"1/2... */
    { FDRIVE_DRV_144, FDRIVE_DISK_720,  9, 80, 0,  "360 kB 3\"1/2", },
    /* end */
    { FDRIVE_DRV_NONE, FDRIVE_DISK_NONE, -1, -1, 0, NULL, },
};

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

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

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

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

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

/********************************************************/
bellard's avatar
bellard committed
313
/* Intel 82078 floppy disk controller emulation          */
314

315
316
static void fdctrl_reset (fdctrl_t *fdctrl, int do_irq);
static void fdctrl_reset_fifo (fdctrl_t *fdctrl);
bellard's avatar
bellard committed
317
318
static int fdctrl_transfer_handler (void *opaque, int nchan,
                                    int dma_pos, int dma_len);
319
static void fdctrl_raise_irq (fdctrl_t *fdctrl, uint8_t status);
320
static void fdctrl_result_timer(void *opaque);
321
322
323
324
325
326
327
328
329
330
331

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

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

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

enum {
    FD_STATE_CMD    = 0x00,
    FD_STATE_STATUS = 0x01,
    FD_STATE_DATA   = 0x02,
    FD_STATE_STATE  = 0x03,
    FD_STATE_MULTI  = 0x10,
    FD_STATE_SEEK   = 0x20,
356
    FD_STATE_FORMAT = 0x40,
357
358
359
};

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

366
367
struct fdctrl_t {
    fdctrl_t *fdctrl;
bellard's avatar
bellard committed
368
    /* Controller's identification */
369
370
    uint8_t version;
    /* HW */
pbrook's avatar
pbrook committed
371
    qemu_irq irq;
372
    int dma_chann;
373
    target_phys_addr_t io_base;
bellard's avatar
bellard committed
374
    /* Controller state */
375
    QEMUTimer *result_timer;
376
377
378
379
380
381
382
383
384
385
386
    uint8_t state;
    uint8_t dma_en;
    uint8_t cur_drv;
    uint8_t bootsel;
    /* Command FIFO */
    uint8_t fifo[FD_SECTOR_LEN];
    uint32_t data_pos;
    uint32_t data_len;
    uint8_t data_state;
    uint8_t data_dir;
    uint8_t int_status;
387
    uint8_t eot; /* last wanted sector */
388
389
390
391
392
393
394
395
396
397
398
399
    /* States kept only to be returned back */
    /* Timers state */
    uint8_t timer0;
    uint8_t timer1;
    /* precompensation */
    uint8_t precomp_trk;
    uint8_t config;
    uint8_t lock;
    /* Power down config (also with status regB access mode */
    uint8_t pwrd;
    /* Floppy drives */
    fdrive_t drives[2];
400
401
402
403
404
405
406
};

static uint32_t fdctrl_read (void *opaque, uint32_t reg)
{
    fdctrl_t *fdctrl = opaque;
    uint32_t retval;

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

    return retval;
}

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

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

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

bellard's avatar
bellard committed
465
466
static uint32_t fdctrl_read_mem (void *opaque, target_phys_addr_t reg)
{
467
    return fdctrl_read(opaque, (uint32_t)reg);
bellard's avatar
bellard committed
468
469
470
471
472
}

static void fdctrl_write_mem (void *opaque, 
                              target_phys_addr_t reg, uint32_t value)
{
473
    fdctrl_write(opaque, (uint32_t)reg, value);
bellard's avatar
bellard committed
474
475
}

bellard's avatar
bellard committed
476
static CPUReadMemoryFunc *fdctrl_mem_read[3] = {
bellard's avatar
bellard committed
477
478
479
    fdctrl_read_mem,
    fdctrl_read_mem,
    fdctrl_read_mem,
bellard's avatar
bellard committed
480
481
482
};

static CPUWriteMemoryFunc *fdctrl_mem_write[3] = {
bellard's avatar
bellard committed
483
484
485
    fdctrl_write_mem,
    fdctrl_write_mem,
    fdctrl_write_mem,
bellard's avatar
bellard committed
486
487
};

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

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

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

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

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

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

    return 0;
}

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

    if (version_id != 1)
        return -EINVAL;

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

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

    return ret;
}

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

    fdctrl_reset(s, 0);
}

pbrook's avatar
pbrook committed
581
fdctrl_t *fdctrl_init (qemu_irq irq, int dma_chann, int mem_mapped, 
582
                       target_phys_addr_t io_base,
583
                       BlockDriverState **fds)
584
{
585
    fdctrl_t *fdctrl;
bellard's avatar
bellard committed
586
    int io_mem;
587
588
    int i;

bellard's avatar
bellard committed
589
    FLOPPY_DPRINTF("init controller\n");
590
591
592
    fdctrl = qemu_mallocz(sizeof(fdctrl_t));
    if (!fdctrl)
        return NULL;
593
594
595
    fdctrl->result_timer = qemu_new_timer(vm_clock, 
                                          fdctrl_result_timer, fdctrl);

bellard's avatar
bellard committed
596
    fdctrl->version = 0x90; /* Intel 82078 controller */
pbrook's avatar
pbrook committed
597
    fdctrl->irq = irq;
598
599
    fdctrl->dma_chann = dma_chann;
    fdctrl->io_base = io_base;
600
    fdctrl->config = 0x60; /* Implicit seek, polling & FIFO enabled */
601
602
603
    if (fdctrl->dma_chann != -1) {
        fdctrl->dma_en = 1;
        DMA_register_channel(dma_chann, &fdctrl_transfer_handler, fdctrl);
604
    } else {
605
        fdctrl->dma_en = 0;
606
    }
607
608
    for (i = 0; i < 2; i++) {
        fd_init(&fdctrl->drives[i], fds[i]);
609
    }
610
611
    fdctrl_reset(fdctrl, 0);
    fdctrl->state = FD_CTRL_ACTIVE;
612
    if (mem_mapped) {
bellard's avatar
bellard committed
613
614
        io_mem = cpu_register_io_memory(0, fdctrl_mem_read, fdctrl_mem_write, fdctrl);
        cpu_register_physical_memory(io_base, 0x08, io_mem);
615
    } else {
616
617
618
619
620
621
622
623
        register_ioport_read((uint32_t)io_base + 0x01, 5, 1, &fdctrl_read,
                             fdctrl);
        register_ioport_read((uint32_t)io_base + 0x07, 1, 1, &fdctrl_read,
                             fdctrl);
        register_ioport_write((uint32_t)io_base + 0x01, 5, 1, &fdctrl_write,
                              fdctrl);
        register_ioport_write((uint32_t)io_base + 0x07, 1, 1, &fdctrl_write,
                              fdctrl);
624
    }
625
626
    register_savevm("fdc", io_base, 1, fdc_save, fdc_load, fdctrl);
    qemu_register_reset(fdctrl_external_reset, fdctrl);
627
    for (i = 0; i < 2; i++) {
628
        fd_revalidate(&fdctrl->drives[i]);
629
    }
630

631
    return fdctrl;
632
}
633

634
635
/* XXX: may change if moved to bdrv */
int fdctrl_get_drive_type(fdctrl_t *fdctrl, int drive_num)
636
{
637
    return fdctrl->drives[drive_num].drive;
638
639
640
}

/* Change IRQ state */
641
static void fdctrl_reset_irq (fdctrl_t *fdctrl)
642
{
643
    FLOPPY_DPRINTF("Reset interrupt\n");
pbrook's avatar
pbrook committed
644
    qemu_set_irq(fdctrl->irq, 0);
645
    fdctrl->state &= ~FD_CTRL_INTR;
646
647
}

648
static void fdctrl_raise_irq (fdctrl_t *fdctrl, uint8_t status)
649
{
bellard's avatar
bellard committed
650
651
652
653
654
655
656
657
#ifdef TARGET_SPARC
    // Sparc mutation
    if (!fdctrl->dma_en) {
	fdctrl->state &= ~FD_CTRL_BUSY;
	fdctrl->int_status = status;
	return;
    }
#endif
658
    if (~(fdctrl->state & FD_CTRL_INTR)) {
pbrook's avatar
pbrook committed
659
        qemu_set_irq(fdctrl->irq, 1);
660
        fdctrl->state |= FD_CTRL_INTR;
661
662
    }
    FLOPPY_DPRINTF("Set interrupt status to 0x%02x\n", status);
663
    fdctrl->int_status = status;
664
665
}

bellard's avatar
bellard committed
666
/* Reset controller */
667
static void fdctrl_reset (fdctrl_t *fdctrl, int do_irq)
668
669
670
{
    int i;

bellard's avatar
bellard committed
671
    FLOPPY_DPRINTF("reset controller\n");
672
    fdctrl_reset_irq(fdctrl);
bellard's avatar
bellard committed
673
    /* Initialise controller */
674
    fdctrl->cur_drv = 0;
675
    /* FIFO state */
676
677
678
679
    fdctrl->data_pos = 0;
    fdctrl->data_len = 0;
    fdctrl->data_state = FD_STATE_CMD;
    fdctrl->data_dir = FD_DIR_WRITE;
680
    for (i = 0; i < MAX_FD; i++)
681
682
        fd_reset(&fdctrl->drives[i]);
    fdctrl_reset_fifo(fdctrl);
683
    if (do_irq)
684
        fdctrl_raise_irq(fdctrl, 0xc0);
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
}

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);
700
701
702
}

/* Status B register : 0x01 (read-only) */
703
static uint32_t fdctrl_read_statusB (fdctrl_t *fdctrl)
704
705
706
707
708
709
{
    FLOPPY_DPRINTF("status register: 0x00\n");
    return 0;
}

/* Digital output register : 0x02 */
710
static uint32_t fdctrl_read_dor (fdctrl_t *fdctrl)
711
712
713
714
{
    uint32_t retval = 0;

    /* Drive motors state indicators */
715
716
717
718
    if (drv0(fdctrl)->drflags & FDRIVE_MOTOR_ON)
	retval |= 1 << 5;
    if (drv1(fdctrl)->drflags & FDRIVE_MOTOR_ON)
	retval |= 1 << 4;
719
    /* DMA enable */
720
    retval |= fdctrl->dma_en << 3;
721
    /* Reset indicator */
722
    retval |= (fdctrl->state & FD_CTRL_RESET) == 0 ? 0x04 : 0;
723
    /* Selected drive */
724
    retval |= fdctrl->cur_drv;
725
726
727
728
729
    FLOPPY_DPRINTF("digital output register: 0x%02x\n", retval);

    return retval;
}

730
static void fdctrl_write_dor (fdctrl_t *fdctrl, uint32_t value)
731
732
{
    /* Reset mode */
733
    if (fdctrl->state & FD_CTRL_RESET) {
734
        if (!(value & 0x04)) {
bellard's avatar
bellard committed
735
            FLOPPY_DPRINTF("Floppy controller in RESET state !\n");
736
737
738
739
740
741
            return;
        }
    }
    FLOPPY_DPRINTF("digital output register set to 0x%02x\n", value);
    /* Drive motors state indicators */
    if (value & 0x20)
742
        fd_start(drv1(fdctrl));
743
    else
744
        fd_stop(drv1(fdctrl));
745
    if (value & 0x10)
746
        fd_start(drv0(fdctrl));
747
    else
748
        fd_stop(drv0(fdctrl));
749
750
    /* DMA enable */
#if 0
751
752
    if (fdctrl->dma_chann != -1)
        fdctrl->dma_en = 1 - ((value >> 3) & 1);
753
754
755
#endif
    /* Reset */
    if (!(value & 0x04)) {
756
        if (!(fdctrl->state & FD_CTRL_RESET)) {
bellard's avatar
bellard committed
757
            FLOPPY_DPRINTF("controller enter RESET state\n");
758
            fdctrl->state |= FD_CTRL_RESET;
759
760
        }
    } else {
761
        if (fdctrl->state & FD_CTRL_RESET) {
bellard's avatar
bellard committed
762
            FLOPPY_DPRINTF("controller out of RESET state\n");
763
            fdctrl_reset(fdctrl, 1);
764
            fdctrl->state &= ~(FD_CTRL_RESET | FD_CTRL_SLEEP);
765
766
767
        }
    }
    /* Selected drive */
768
    fdctrl->cur_drv = value & 1;
769
770
771
}

/* Tape drive register : 0x03 */
772
static uint32_t fdctrl_read_tape (fdctrl_t *fdctrl)
773
774
775
776
{
    uint32_t retval = 0;

    /* Disk boot selection indicator */
777
    retval |= fdctrl->bootsel << 2;
778
779
780
781
782
783
    /* Tape indicators: never allowed */
    FLOPPY_DPRINTF("tape drive register: 0x%02x\n", retval);

    return retval;
}

784
static void fdctrl_write_tape (fdctrl_t *fdctrl, uint32_t value)
785
786
{
    /* Reset mode */
787
    if (fdctrl->state & FD_CTRL_RESET) {
bellard's avatar
bellard committed
788
        FLOPPY_DPRINTF("Floppy controller in RESET state !\n");
789
790
791
792
        return;
    }
    FLOPPY_DPRINTF("tape drive register set to 0x%02x\n", value);
    /* Disk boot selection indicator */
793
    fdctrl->bootsel = (value >> 2) & 1;
794
795
796
797
    /* Tape indicators: never allow */
}

/* Main status register : 0x04 (read) */
798
static uint32_t fdctrl_read_main_status (fdctrl_t *fdctrl)
799
800
801
{
    uint32_t retval = 0;

802
803
    fdctrl->state &= ~(FD_CTRL_SLEEP | FD_CTRL_RESET);
    if (!(fdctrl->state & FD_CTRL_BUSY)) {
804
805
806
        /* Data transfer allowed */
        retval |= 0x80;
        /* Data transfer direction indicator */
807
        if (fdctrl->data_dir == FD_DIR_READ)
808
809
810
811
            retval |= 0x40;
    }
    /* Should handle 0x20 for SPECIFY command */
    /* Command busy indicator */
812
813
    if (FD_STATE(fdctrl->data_state) == FD_STATE_DATA ||
        FD_STATE(fdctrl->data_state) == FD_STATE_STATUS)
814
815
816
817
818
819
820
        retval |= 0x10;
    FLOPPY_DPRINTF("main status register: 0x%02x\n", retval);

    return retval;
}

/* Data select rate register : 0x04 (write) */
821
static void fdctrl_write_rate (fdctrl_t *fdctrl, uint32_t value)
822
823
{
    /* Reset mode */
824
    if (fdctrl->state & FD_CTRL_RESET) {
bellard's avatar
bellard committed
825
            FLOPPY_DPRINTF("Floppy controller in RESET state !\n");
826
827
828
829
830
            return;
        }
    FLOPPY_DPRINTF("select rate register set to 0x%02x\n", value);
    /* Reset: autoclear */
    if (value & 0x80) {
831
832
833
        fdctrl->state |= FD_CTRL_RESET;
        fdctrl_reset(fdctrl, 1);
        fdctrl->state &= ~FD_CTRL_RESET;
834
835
    }
    if (value & 0x40) {
836
837
        fdctrl->state |= FD_CTRL_SLEEP;
        fdctrl_reset(fdctrl, 1);
838
839
840
841
    }
//        fdctrl.precomp = (value >> 2) & 0x07;
}

bellard's avatar
bellard committed
842
843
844
845
846
847
848
849
850
851
852
853
static int fdctrl_media_changed(fdrive_t *drv)
{
    int ret;
    if (!drv->bs) 
        return 0;
    ret = bdrv_media_changed(drv->bs);
    if (ret) {
        fd_revalidate(drv);
    }
    return ret;
}

854
/* Digital input register : 0x07 (read-only) */
855
static uint32_t fdctrl_read_dir (fdctrl_t *fdctrl)
856
857
858
{
    uint32_t retval = 0;

bellard's avatar
bellard committed
859
860
    if (fdctrl_media_changed(drv0(fdctrl)) ||
	fdctrl_media_changed(drv1(fdctrl)))
861
862
        retval |= 0x80;
    if (retval != 0)
863
        FLOPPY_DPRINTF("Floppy digital input register: 0x%02x\n", retval);
864
865
866
867
868

    return retval;
}

/* FIFO state control */
869
static void fdctrl_reset_fifo (fdctrl_t *fdctrl)
870
{
871
872
873
    fdctrl->data_dir = FD_DIR_WRITE;
    fdctrl->data_pos = 0;
    FD_SET_STATE(fdctrl->data_state, FD_STATE_CMD);
874
875
876
}

/* Set FIFO status for the host to read */
877
static void fdctrl_set_fifo (fdctrl_t *fdctrl, int fifo_len, int do_irq)
878
{
879
880
881
882
    fdctrl->data_dir = FD_DIR_READ;
    fdctrl->data_len = fifo_len;
    fdctrl->data_pos = 0;
    FD_SET_STATE(fdctrl->data_state, FD_STATE_STATUS);
883
    if (do_irq)
884
        fdctrl_raise_irq(fdctrl, 0x00);
885
886
887
}

/* Set an error: unimplemented/unknown command */
888
static void fdctrl_unimplemented (fdctrl_t *fdctrl)
889
890
{
#if 0
891
892
893
    fdrive_t *cur_drv;

    cur_drv = get_cur_drv(fdctrl);
894
    fdctrl->fifo[0] = 0x60 | (cur_drv->head << 2) | fdctrl->cur_drv;
895
896
897
    fdctrl->fifo[1] = 0x00;
    fdctrl->fifo[2] = 0x00;
    fdctrl_set_fifo(fdctrl, 3, 1);
898
#else
899
900
901
    //    fdctrl_reset_fifo(fdctrl);
    fdctrl->fifo[0] = 0x80;
    fdctrl_set_fifo(fdctrl, 1, 0);
902
903
904
905
#endif
}

/* Callback for transfer end (stop or abort) */
906
907
static void fdctrl_stop_transfer (fdctrl_t *fdctrl, uint8_t status0,
				  uint8_t status1, uint8_t status2)
908
{
909
    fdrive_t *cur_drv;
910

911
    cur_drv = get_cur_drv(fdctrl);
912
913
    FLOPPY_DPRINTF("transfer status: %02x %02x %02x (%02x)\n",
                   status0, status1, status2,
914
915
                   status0 | (cur_drv->head << 2) | fdctrl->cur_drv);
    fdctrl->fifo[0] = status0 | (cur_drv->head << 2) | fdctrl->cur_drv;
916
917
918
919
920
921
922
    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;
923
    if (fdctrl->state & FD_CTRL_BUSY) {
924
        DMA_release_DREQ(fdctrl->dma_chann);
925
926
        fdctrl->state &= ~FD_CTRL_BUSY;
    }
927
    fdctrl_set_fifo(fdctrl, 7, 1);
928
929
930
}

/* Prepare a data transfer (either DMA or FIFO) */
931
static void fdctrl_start_transfer (fdctrl_t *fdctrl, int direction)
932
{
933
    fdrive_t *cur_drv;
934
935
936
    uint8_t kh, kt, ks;
    int did_seek;

937
938
939
940
941
    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's avatar
bellard committed
942
    FLOPPY_DPRINTF("Start transfer at %d %d %02x %02x (%d)\n",
943
                   fdctrl->cur_drv, kh, kt, ks,
944
945
                   _fd_sector(kh, kt, ks, cur_drv->last_sect));
    did_seek = 0;
946
    switch (fd_seek(cur_drv, kh, kt, ks, fdctrl->config & 0x40)) {
947
948
    case 2:
        /* sect too big */
949
950
951
952
        fdctrl_stop_transfer(fdctrl, 0x40, 0x00, 0x00);
        fdctrl->fifo[3] = kt;
        fdctrl->fifo[4] = kh;
        fdctrl->fifo[5] = ks;
953
954
955
        return;
    case 3:
        /* track too big */
956
957
958
959
        fdctrl_stop_transfer(fdctrl, 0x40, 0x80, 0x00);
        fdctrl->fifo[3] = kt;
        fdctrl->fifo[4] = kh;
        fdctrl->fifo[5] = ks;
960
961
962
        return;
    case 4:
        /* No seek enabled */
963
964
965
966
        fdctrl_stop_transfer(fdctrl, 0x40, 0x00, 0x00);
        fdctrl->fifo[3] = kt;
        fdctrl->fifo[4] = kh;
        fdctrl->fifo[5] = ks;
967
968
969
970
971
972
973
974
        return;
    case 1:
        did_seek = 1;
        break;
    default:
        break;
    }
    /* Set the FIFO state */
975
976
977
978
979
980
981
    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;
982
    if (did_seek)
983
984
985
986
987
988
989
        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;
ths's avatar
ths committed
990
        fdctrl->data_len = 128 << (fdctrl->fifo[5] > 7 ? 7 : fdctrl->fifo[5]);
991
992
993
994
995
        tmp = (cur_drv->last_sect - ks + 1);
        if (fdctrl->fifo[0] & 0x80)
            tmp += cur_drv->last_sect;
	fdctrl->data_len *= tmp;
    }
996
    fdctrl->eot = fdctrl->fifo[6];
997
    if (fdctrl->dma_en) {
998
999
        int dma_mode;
        /* DMA transfer are enabled. Check if DMA channel is well programmed */
1000
        dma_mode = DMA_get_channel_mode(fdctrl->dma_chann);
1001
        dma_mode = (dma_mode >> 2) & 3;
1002
1003
1004
1005
        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);
1006
1007
1008
1009
1010
        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 */
1011
            fdctrl->state |= FD_CTRL_BUSY;
bellard's avatar
bellard committed
1012
            /* Now, we just have to wait for the DMA controller to
1013
1014
             * recall us...
             */
1015
1016
            DMA_hold_DREQ(fdctrl->dma_chann);
            DMA_schedule(fdctrl->dma_chann);
1017
            return;
1018
1019
        } else {
	    FLOPPY_ERROR("dma_mode=%d direction=%d\n", dma_mode, direction);
1020
1021
1022
1023
        }
    }
    FLOPPY_DPRINTF("start non-DMA transfer\n");
    /* IO based transfer: calculate len */
1024
    fdctrl_raise_irq(fdctrl, 0x00);
1025
1026
1027
1028
1029

    return;
}

/* Prepare a transfer of deleted data */
1030
static void fdctrl_start_transfer_del (fdctrl_t *fdctrl, int direction)
1031
1032
1033
1034
{
    /* We don't handle deleted data,
     * so we don't return *ANYTHING*
     */
1035
    fdctrl_stop_transfer(fdctrl, 0x60, 0x00, 0x00);
1036
1037
1038
}

/* handlers for DMA transfers */
bellard's avatar
bellard committed
1039
1040
static int fdctrl_transfer_handler (void *opaque, int nchan,
                                    int dma_pos, int dma_len)
1041
{
1042
1043
1044
    fdctrl_t *fdctrl;
    fdrive_t *cur_drv;
    int len, start_pos, rel_pos;
1045
1046
    uint8_t status0 = 0x00, status1 = 0x00, status2 = 0x00;

1047
1048
    fdctrl = opaque;
    if (!(fdctrl->state & FD_CTRL_BUSY)) {
1049
1050
1051
        FLOPPY_DPRINTF("Not in DMA transfer mode !\n");
        return 0;
    }
1052
1053
1054
    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)
1055
        status2 = 0x04;
bellard's avatar
bellard committed
1056
1057
    if (dma_len > fdctrl->data_len)
        dma_len = fdctrl->data_len;
1058
    if (cur_drv->bs == NULL) {
1059
1060
1061
1062
1063
	if (fdctrl->data_dir == FD_DIR_WRITE)
	    fdctrl_stop_transfer(fdctrl, 0x60, 0x00, 0x00);
	else
	    fdctrl_stop_transfer(fdctrl, 0x40, 0x00, 0x00);
	len = 0;
1064
1065
        goto transfer_error;
    }
1066
    rel_pos = fdctrl->data_pos % FD_SECTOR_LEN;
bellard's avatar
bellard committed
1067
1068
    for (start_pos = fdctrl->data_pos; fdctrl->data_pos < dma_len;) {
        len = dma_len - fdctrl->data_pos;
1069
1070
        if (len + rel_pos > FD_SECTOR_LEN)
            len = FD_SECTOR_LEN - rel_pos;
bellard's avatar
bellard committed
1071
1072
        FLOPPY_DPRINTF("copy %d bytes (%d %d %d) %d pos %d %02x "
                       "(%d-0x%08x 0x%08x)\n", len, dma_len, fdctrl->data_pos,
1073
1074
                       fdctrl->data_len, fdctrl->cur_drv, cur_drv->head,
                       cur_drv->track, cur_drv->sect, fd_sector(cur_drv),
bellard's avatar
bellard committed
1075
                       fd_sector(cur_drv) * 512);
1076
1077
1078
1079
1080
        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) {
1081
1082
1083
                FLOPPY_DPRINTF("Floppy: error getting sector %d\n",
                               fd_sector(cur_drv));
                /* Sure, image size is too small... */
1084
                memset(fdctrl->fifo, 0, FD_SECTOR_LEN);
1085
            }
1086
        }
1087
1088
1089
	switch (fdctrl->data_dir) {
	case FD_DIR_READ:
	    /* READ commands */
bellard's avatar
bellard committed
1090
1091
1092
1093
            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); */
1094
1095
1096
	    break;
	case FD_DIR_WRITE:
            /* WRITE commands */
bellard's avatar
bellard committed
1097
1098
1099
1100
            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); */
1101
1102
1103
1104
1105
            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;
1106
            }
1107
1108
1109
1110
1111
1112
	    break;
	default:
	    /* SCAN commands */
            {
		uint8_t tmpbuf[FD_SECTOR_LEN];
                int ret;
bellard's avatar
bellard committed
1113
1114
1115
                DMA_read_memory (nchan, tmpbuf, fdctrl->data_pos, len);
/*                 cpu_physical_memory_read(addr + fdctrl->data_pos, */
/*                                          tmpbuf, len); */
1116
                ret = memcmp(tmpbuf, fdctrl->fifo + rel_pos, len);
1117
1118
1119
1120
                if (ret == 0) {
                    status2 = 0x08;
                    goto end_transfer;
                }
1121
1122
                if ((ret < 0 && fdctrl->data_dir == FD_DIR_SCANL) ||
                    (ret > 0 && fdctrl->data_dir == FD_DIR_SCANH)) {
1123
1124
1125
1126
                    status2 = 0x00;
                    goto end_transfer;
                }
            }
1127
	    break;
1128
        }
1129
1130
1131
	fdctrl->data_pos += len;
	rel_pos = fdctrl->data_pos % FD_SECTOR_LEN;
        if (rel_pos == 0) {
1132
            /* Seek to next sector */
1133
1134
1135
	    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's avatar
bellard committed
1136
			   fdctrl->data_pos - len);
1137
1138
1139
1140
            /* 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) {
1141
1142
1143
1144
		cur_drv->sect = 1;
		if (FD_MULTI_TRACK(fdctrl->data_state)) {
		    if (cur_drv->head == 0 &&
			(cur_drv->flags & FDISK_DBL_SIDES) != 0) {	
1145
1146
1147
                        cur_drv->head = 1;
                    } else {
                        cur_drv->head = 0;
1148
1149
1150
			cur_drv->track++;
			if ((cur_drv->flags & FDISK_DBL_SIDES) == 0)
			    break;
1151
1152
1153
1154
                    }
                } else {
                    cur_drv->track++;
                    break;
1155
                }
1156
1157
1158
		FLOPPY_DPRINTF("seek to next track (%d %02x %02x => %d)\n",
			       cur_drv->head, cur_drv->track,
			       cur_drv->sect, fd_sector(cur_drv));
1159
1160
            } else {
                cur_drv->sect++;
1161
1162
1163
1164
            }
        }
    }
end_transfer:
1165
1166
1167
1168
1169