Floppy disk emulation (Jocelyn Mayer) (8977f3c1) | Commits | gwj / at91sam9263

Commit 8977f3c107efa48edc19ea45f513f204bfe9d8e7

Authored by bellard 2004-01-05 00:09:06 +0000

Floppy disk emulation (Jocelyn Mayer)


git-svn-id: svn://svn.savannah.nongnu.org/qemu/trunk@545 c046a42c-6fe2-441c-8c8c-71466251a162

Inline Side-by-side

Showing 1 changed file with 1435 additions and 0 deletions

hw/fdc.c 0 → 100644

View file @8977f3c

	1	+/*
	2	+ * QEMU Floppy disk emulator
	3	+ *
	4	+ * Copyright (c) 2003 Jocelyn Mayer
	5	+ *
	6	+ * Permission is hereby granted, free of charge, to any person obtaining a copy
	7	+ * of this software and associated documentation files (the "Software"), to deal
	8	+ * in the Software without restriction, including without limitation the rights
	9	+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
	10	+ * copies of the Software, and to permit persons to whom the Software is
	11	+ * furnished to do so, subject to the following conditions:
	12	+ *
	13	+ * The above copyright notice and this permission notice shall be included in
	14	+ * all copies or substantial portions of the Software.
	15	+ *
	16	+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
	17	+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
	18	+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
	19	+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
	20	+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
	21	+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
	22	+ * THE SOFTWARE.
	23	+ */
	24	+#include <stdio.h>
	25	+#include <stdlib.h>
	26	+#include <string.h>
	27	+#include <inttypes.h>
	28	+
	29	+#include "cpu.h"
	30	+#include "vl.h"
	31	+
	32	+/********************************************************/
	33	+/* debug Floppy devices */
	34	+//#define DEBUG_FLOPPY
	35	+
	36	+#ifdef DEBUG_FLOPPY
	37	+#define FLOPPY_DPRINTF(fmt, args...) \
	38	+do { printf("FLOPPY: " fmt , ##args); } while (0)
	39	+#else
	40	+#define FLOPPY_DPRINTF(fmt, args...)
	41	+#endif
	42	+
	43	+#define FLOPPY_ERROR(fmt, args...) \
	44	+do { printf("FLOPPY ERROR: %s: " fmt, __func__ , ##args); } while (0)
	45	+
	46	+/********************************************************/
	47	+/* Floppy drive emulation */
	48	+
	49	+/* Will always be a fixed parameter for us */
	50	+#define FD_SECTOR_LEN 512
	51	+#define FD_SECTOR_SC 2 /* Sector size code */
	52	+
	53	+/* Floppy disk drive emulation */
	54	+typedef enum fdisk_type_t {
	55	+ FDRIVE_DISK_288 = 0x01, /* 2.88 MB disk */
	56	+ FDRIVE_DISK_144 = 0x02, /* 1.44 MB disk */
	57	+ FDRIVE_DISK_720 = 0x03, /* 720 kB disk */
	58	+ FDRIVE_DISK_NONE = 0x04, /* No disk */
	59	+} fdisk_type_t;
	60	+
	61	+typedef enum fdrive_type_t {
	62	+ FDRIVE_DRV_144 = 0x00, /* 1.44 MB 3"5 drive */
	63	+ FDRIVE_DRV_288 = 0x01, /* 2.88 MB 3"5 drive */
	64	+ FDRIVE_DRV_120 = 0x02, /* 1.2 MB 5"25 drive */
	65	+ FDRIVE_DRV_NONE = 0x03, /* No drive connected */
	66	+} fdrive_type_t;
	67	+
	68	+typedef struct fdrive_t {
	69	+ BlockDriverState *bs;
	70	+ /* Drive status */
	71	+ fdrive_type_t drive;
	72	+ uint8_t motor; /* on/off */
	73	+ uint8_t perpendicular; /* 2.88 MB access mode */
	74	+ uint8_t rv; /* Revalidated */
	75	+ /* Position */
	76	+ uint8_t head;
	77	+ uint8_t track;
	78	+ uint8_t sect;
	79	+ /* Last operation status */
	80	+ uint8_t dir; /* Direction */
	81	+ uint8_t rw; /* Read/write */
	82	+ /* Media */
	83	+ fdisk_type_t disk; /* Disk type */
	84	+ uint8_t last_sect; /* Nb sector per track */
	85	+ uint8_t max_track; /* Nb of tracks */
	86	+ uint8_t ro; /* Is read-only */
	87	+} fdrive_t;
	88	+
	89	+static void fd_init (fdrive_t *drv)
	90	+{
	91	+ /* Drive */
	92	+ drv->bs = NULL;
	93	+// drv->drive = FDRIVE_DRV_288;
	94	+ drv->drive = FDRIVE_DRV_144;
	95	+ drv->motor = 0;
	96	+ drv->perpendicular = 0;
	97	+ drv->rv = 0;
	98	+ /* Disk */
	99	+ drv->disk = FDRIVE_DISK_NONE;
	100	+ drv->last_sect = 1;
	101	+ drv->max_track = 0;
	102	+}
	103	+
	104	+static int _fd_sector (uint8_t head, uint8_t track,
	105	+ uint8_t sect, uint8_t last_sect)
	106	+{
	107	+ return (((track * 2) + head) * last_sect) + sect - 1;
	108	+}
	109	+
	110	+/* Returns current position, in sectors, for given drive */
	111	+static int fd_sector (fdrive_t *drv)
	112	+{
	113	+ return _fd_sector(drv->head, drv->track, drv->sect, drv->last_sect);
	114	+}
	115	+
	116	+static int fd_seek (fdrive_t *drv, uint8_t head, uint8_t track, uint8_t sect,
	117	+ int enable_seek)
	118	+{
	119	+ uint32_t sector;
	120	+
	121	+ if (track > drv->max_track) {
	122	+ FLOPPY_ERROR("try to read %d %02x %02x (max=%d %02x %02x)\n",
	123	+ head, track, sect, 1, drv->max_track, drv->last_sect);
	124	+ return 2;
	125	+ }
	126	+ if (sect > drv->last_sect) {
	127	+ FLOPPY_ERROR("try to read %d %02x %02x (max=%d %02x %02x)\n",
	128	+ head, track, sect, 1, drv->max_track, drv->last_sect);
	129	+ return 3;
	130	+ }
	131	+ sector = _fd_sector(head, track, sect, drv->last_sect);
	132	+ if (sector != fd_sector(drv)) {
	133	+#if 0
	134	+ if (!enable_seek) {
	135	+ FLOPPY_ERROR("no implicit seek %d %02x %02x (max=%d %02x %02x)\n",
	136	+ head, track, sect, 1, drv->max_track, drv->last_sect);
	137	+ return 4;
	138	+ }
	139	+#endif
	140	+ drv->head = head;
	141	+ drv->track = track;
	142	+ drv->sect = sect;
	143	+ return 1;
	144	+ }
	145	+
	146	+ return 0;
	147	+}
	148	+
	149	+/* Set drive back to track 0 */
	150	+static void fd_recalibrate (fdrive_t *drv)
	151	+{
	152	+ FLOPPY_DPRINTF("recalibrate\n");
	153	+ drv->head = 0;
	154	+ drv->track = 0;
	155	+ drv->sect = 1;
	156	+ drv->dir = 1;
	157	+ drv->rw = 0;
	158	+}
	159	+
	160	+/* Revalidate a disk drive after a disk change */
	161	+static void fd_revalidate (fdrive_t *drv, int ro)
	162	+{
	163	+ int64_t nb_sectors;
	164	+
	165	+ FLOPPY_DPRINTF("revalidate\n");
	166	+ drv->rv = 0;
	167	+ if (drv->bs != NULL) {
	168	+ bdrv_get_geometry(drv->bs, &nb_sectors);
	169	+#if 1
	170	+ if (nb_sectors > 2880)
	171	+#endif
	172	+ {
	173	+ /* Pretend we have a 2.88 MB disk */
	174	+ drv->disk = FDRIVE_DISK_288;
	175	+ drv->last_sect = 36;
	176	+ drv->max_track = 80;
	177	+#if 1
	178	+ } else if (nb_sectors > 1440) {
	179	+ /* Pretend we have a 1.44 MB disk */
	180	+ drv->disk = FDRIVE_DISK_144;
	181	+ drv->last_sect = 18;
	182	+ drv->max_track = 80;
	183	+ } else {
	184	+ /* Pretend we have a 720 kB disk */
	185	+ drv->disk = FDRIVE_DISK_720;
	186	+ drv->last_sect = 9;
	187	+ drv->max_track = 80;
	188	+#endif
	189	+ }
	190	+ } else {
	191	+ drv->disk = FDRIVE_DISK_NONE;
	192	+ drv->last_sect = 1; /* Avoid eventual divide by 0 bugs */
	193	+ }
	194	+ drv->ro = ro;
	195	+ drv->rv = 1;
	196	+}
	197	+
	198	+/* Motor control */
	199	+static void fd_start (fdrive_t *drv)
	200	+{
	201	+ drv->motor = 1;
	202	+}
	203	+
	204	+static void fd_stop (fdrive_t *drv)
	205	+{
	206	+ drv->motor = 0;
	207	+}
	208	+
	209	+/* Re-initialise a drives (motor off, repositioned) */
	210	+static void fd_reset (fdrive_t *drv)
	211	+{
	212	+ fd_stop(drv);
	213	+ fd_recalibrate(drv);
	214	+}
	215	+
	216	+/********************************************************/
	217	+/* Intel 82078 floppy disk controler emulation */
	218	+
	219	+static void fdctrl_reset (int do_irq);
	220	+static void fdctrl_reset_fifo (void);
	221	+static int fdctrl_transfer_handler (uint32_t addr, int size, int *irq);
	222	+static int fdctrl_misc_handler (int duknwo);
	223	+static void fdctrl_raise_irq (uint8_t status);
	224	+
	225	+static uint32_t fdctrl_read_statusB (CPUState *env, uint32_t reg);
	226	+static uint32_t fdctrl_read_dor (CPUState *env, uint32_t reg);
	227	+static void fdctrl_write_dor (CPUState *env, uint32_t reg, uint32_t value);
	228	+static uint32_t fdctrl_read_tape (CPUState *env, uint32_t reg);
	229	+static void fdctrl_write_tape (CPUState *env, uint32_t reg, uint32_t value);
	230	+static uint32_t fdctrl_read_main_status (CPUState *env, uint32_t reg);
	231	+static void fdctrl_write_rate (CPUState *env, uint32_t reg, uint32_t value);
	232	+static uint32_t fdctrl_read_data (CPUState *env, uint32_t reg);
	233	+static void fdctrl_write_data (CPUState *env, uint32_t reg, uint32_t value);
	234	+static uint32_t fdctrl_read_dir (CPUState *env, uint32_t reg);
	235	+
	236	+enum {
	237	+ FD_CTRL_ACTIVE = 0x01,
	238	+ FD_CTRL_RESET = 0x02,
	239	+ FD_CTRL_SLEEP = 0x04,
	240	+ FD_CTRL_BUSY = 0x08,
	241	+ FD_CTRL_INTR = 0x10,
	242	+};
	243	+
	244	+enum {
	245	+ FD_DIR_WRITE = 0,
	246	+ FD_DIR_READ = 1,
	247	+ FD_DIR_SCANE = 2,
	248	+ FD_DIR_SCANL = 3,
	249	+ FD_DIR_SCANH = 4,
	250	+};
	251	+
	252	+enum {
	253	+ FD_STATE_CMD = 0x00,
	254	+ FD_STATE_STATUS = 0x01,
	255	+ FD_STATE_DATA = 0x02,
	256	+ FD_STATE_STATE = 0x03,
	257	+ FD_STATE_MULTI = 0x10,
	258	+ FD_STATE_SEEK = 0x20,
	259	+};
	260	+
	261	+#define FD_STATE(state) ((state) & FD_STATE_STATE)
	262	+#define FD_MULTI_TRACK(state) ((state) & FD_STATE_MULTI)
	263	+#define FD_DID_SEEK(state) ((state) & FD_STATE_SEEK)
	264	+
	265	+typedef struct fdctrl_t {
	266	+ /* Controler's identification */
	267	+ uint8_t version;
	268	+ /* HW */
	269	+ int irq_lvl;
	270	+ int dma_chann;
	271	+ /* Controler state */
	272	+ uint8_t state;
	273	+ uint8_t dma_en;
	274	+ uint8_t cur_drv;
	275	+ uint8_t bootsel;
	276	+ /* Command FIFO */
	277	+ uint8_t fifo[FD_SECTOR_LEN];
	278	+ uint32_t data_pos;
	279	+ uint32_t data_len;
	280	+ uint8_t data_state;
	281	+ uint8_t data_dir;
	282	+ uint8_t int_status;
	283	+ /* States kept only to be returned back */
	284	+ /* Timers state */
	285	+ uint8_t timer0;
	286	+ uint8_t timer1;
	287	+ /* precompensation */
	288	+ uint8_t precomp_trk;
	289	+ uint8_t config;
	290	+ uint8_t lock;
	291	+ /* Power down config (also with status regB access mode */
	292	+ uint8_t pwrd;
	293	+ /* Floppy drives */
	294	+ fdrive_t drives[2];
	295	+} fdctrl_t;
	296	+
	297	+static fdctrl_t fdctrl;
	298	+
	299	+void fdctrl_init (int irq_lvl, int dma_chann, int mem_mapped, uint32_t base,
	300	+ char boot_device)
	301	+{
	302	+// int io_mem;
	303	+ int i;
	304	+
	305	+ FLOPPY_DPRINTF("init controler\n");
	306	+ memset(&fdctrl, 0, sizeof(fdctrl));
	307	+ fdctrl.version = 0x90; /* Intel 82078 controler */
	308	+ fdctrl.irq_lvl = irq_lvl;
	309	+ fdctrl.dma_chann = dma_chann;
	310	+ fdctrl.config = 0x40; /* Implicit seek, polling & FIFO enabled */
	311	+ if (fdctrl.dma_chann != -1) {
	312	+ fdctrl.dma_en = 1;
	313	+ DMA_register_channel(dma_chann, &fdctrl_transfer_handler,
	314	+ &fdctrl_misc_handler);
	315	+ } else {
	316	+ fdctrl.dma_en = 0;
	317	+ }
	318	+ for (i = 0; i < MAX_FD; i++)
	319	+ fd_init(&fdctrl.drives[i]);
	320	+ fdctrl_reset(0);
	321	+ fdctrl.state = FD_CTRL_ACTIVE;
	322	+ if (mem_mapped) {
	323	+ FLOPPY_ERROR("memory mapped floppy not supported by now !\n");
	324	+#if 0
	325	+ io_mem = cpu_register_io_memory(0, fdctrl_mem_read, fdctrl_mem_write);
	326	+ cpu_register_physical_memory(base, 0x08, io_mem);
	327	+#endif
	328	+ } else {
	329	+ register_ioport_read(base + 0x01, 1, fdctrl_read_statusB, 1);
	330	+ register_ioport_read(base + 0x02, 1, fdctrl_read_dor, 1);
	331	+ register_ioport_write(base + 0x02, 1, fdctrl_write_dor, 1);
	332	+ register_ioport_read(base + 0x03, 1, fdctrl_read_tape, 1);
	333	+ register_ioport_write(base + 0x03, 1, fdctrl_write_tape, 1);
	334	+ register_ioport_read(base + 0x04, 1, fdctrl_read_main_status, 1);
	335	+ register_ioport_write(base + 0x04, 1, fdctrl_write_rate, 1);
	336	+ register_ioport_read(base + 0x05, 1, fdctrl_read_data, 1);
	337	+ register_ioport_write(base + 0x05, 1, fdctrl_write_data, 1);
	338	+ register_ioport_read(base + 0x07, 1, fdctrl_read_dir, 1);
	339	+ }
	340	+ if (boot_device == 'b')
	341	+ fdctrl.bootsel = 1;
	342	+ else
	343	+ fdctrl.bootsel = 0;
	344	+#if defined (TARGET_I386)
	345	+ cmos_register_fd(fdctrl.drives[0].drive, fdctrl.drives[1].drive);
	346	+#endif
	347	+}
	348	+
	349	+int fdctrl_disk_change (int idx, const unsigned char *filename, int ro)
	350	+{
	351	+ fdrive_t *drv;
	352	+
	353	+ if (idx < 0 \|\| idx > 1)
	354	+ return -1;
	355	+ FLOPPY_DPRINTF("disk %d change: %s (%s)\n", idx, filename,
	356	+ ro == 0 ? "rw" : "ro");
	357	+ drv = &fdctrl.drives[idx];
	358	+ if (fd_table[idx] != NULL) {
	359	+ bdrv_close(fd_table[idx]);
	360	+ fd_table[idx] = NULL;
	361	+ }
	362	+ fd_table[idx] = bdrv_open(filename, ro);
	363	+ drv->bs = fd_table[idx];
	364	+ if (fd_table[idx] == NULL)
	365	+ return -1;
	366	+ fd_revalidate(drv, ro);
	367	+#if 0
	368	+ fd_recalibrate(drv);
	369	+ fdctrl_reset_fifo();
	370	+ fdctrl_raise_irq(0x20);
	371	+#endif
	372	+
	373	+ return 0;
	374	+}
	375	+
	376	+/* Change IRQ state */
	377	+static void fdctrl_reset_irq (void)
	378	+{
	379	+ if (fdctrl.state & FD_CTRL_INTR) {
	380	+ pic_set_irq(fdctrl.irq_lvl, 0);
	381	+ fdctrl.state &= ~(FD_CTRL_INTR \| FD_CTRL_SLEEP \| FD_CTRL_BUSY);
	382	+ }
	383	+}
	384	+
	385	+static void fdctrl_raise_irq (uint8_t status)
	386	+{
	387	+ if (~(fdctrl.state & FD_CTRL_INTR)) {
	388	+ pic_set_irq(fdctrl.irq_lvl, 1);
	389	+ fdctrl.state \|= FD_CTRL_INTR;
	390	+ }
	391	+ FLOPPY_DPRINTF("Set interrupt status to 0x%02x\n", status);
	392	+ fdctrl.int_status = status;
	393	+}
	394	+
	395	+/* Reset controler */
	396	+static void fdctrl_reset (int do_irq)
	397	+{
	398	+ int i;
	399	+
	400	+ FLOPPY_DPRINTF("reset controler\n");
	401	+ fdctrl_reset_irq();
	402	+ /* Initialise controler */
	403	+ fdctrl.cur_drv = 0;
	404	+ /* FIFO state */
	405	+ fdctrl.data_pos = 0;
	406	+ fdctrl.data_len = 0;
	407	+ fdctrl.data_state = FD_STATE_CMD;
	408	+ fdctrl.data_dir = FD_DIR_WRITE;
	409	+ for (i = 0; i < MAX_FD; i++)
	410	+ fd_reset(&fdctrl.drives[i]);
	411	+ fdctrl_reset_fifo();
	412	+ if (do_irq)
	413	+ fdctrl_raise_irq(0x20);
	414	+}
	415	+
	416	+/* Status B register : 0x01 (read-only) */
	417	+static uint32_t fdctrl_read_statusB (CPUState *env, uint32_t reg)
	418	+{
	419	+ fdctrl_reset_irq();
	420	+ FLOPPY_DPRINTF("status register: 0x00\n");
	421	+
	422	+ return 0;
	423	+}
	424	+
	425	+/* Digital output register : 0x02 */
	426	+static uint32_t fdctrl_read_dor (CPUState *env, uint32_t reg)
	427	+{
	428	+ fdrive_t cur_drv, drv0, *drv1;
	429	+ uint32_t retval = 0;
	430	+
	431	+ drv0 = &fdctrl.drives[fdctrl.bootsel];
	432	+ drv1 = &fdctrl.drives[1 - fdctrl.bootsel];
	433	+ cur_drv = fdctrl.cur_drv == 0 ? drv0 : drv1;
	434	+ /* Drive motors state indicators */
	435	+ retval \|= drv1->motor << 5;
	436	+ retval \|= drv0->motor << 4;
	437	+ /* DMA enable */
	438	+ retval \|= fdctrl.dma_en << 3;
	439	+ /* Reset indicator */
	440	+ retval \|= (fdctrl.state & FD_CTRL_RESET) == 0 ? 0x04 : 0;
	441	+ /* Selected drive */
	442	+ retval \|= fdctrl.cur_drv;
	443	+ FLOPPY_DPRINTF("digital output register: 0x%02x\n", retval);
	444	+
	445	+ return retval;
	446	+}
	447	+
	448	+static void fdctrl_write_dor (CPUState *env, uint32_t reg, uint32_t value)
	449	+{
	450	+ fdrive_t drv0, drv1;
	451	+
	452	+ fdctrl_reset_irq();
	453	+ drv0 = &fdctrl.drives[fdctrl.bootsel];
	454	+ drv1 = &fdctrl.drives[1 - fdctrl.bootsel];
	455	+ /* Reset mode */
	456	+ if (fdctrl.state & FD_CTRL_RESET) {
	457	+ if (!(value & 0x04)) {
	458	+ FLOPPY_DPRINTF("Floppy controler in RESET state !\n");
	459	+ return;
	460	+ }
	461	+ }
	462	+ FLOPPY_DPRINTF("digital output register set to 0x%02x\n", value);
	463	+ /* Drive motors state indicators */
	464	+ if (value & 0x20)
	465	+ fd_start(drv1);
	466	+ else
	467	+ fd_stop(drv1);
	468	+ if (value & 0x10)
	469	+ fd_start(drv0);
	470	+ else
	471	+ fd_stop(drv0);
	472	+ /* DMA enable */
	473	+#if 0
	474	+ if (fdctrl.dma_chann != -1)
	475	+ fdctrl.dma_en = 1 - ((value >> 3) & 1);
	476	+#endif
	477	+ /* Reset */
	478	+ if (!(value & 0x04)) {
	479	+ if (!(fdctrl.state & FD_CTRL_RESET)) {
	480	+ FLOPPY_DPRINTF("controler enter RESET state\n");
	481	+ fdctrl.state \|= FD_CTRL_RESET;
	482	+ fdctrl_reset(1);
	483	+ }
	484	+ } else {
	485	+ if (fdctrl.state & FD_CTRL_RESET) {
	486	+ FLOPPY_DPRINTF("controler out of RESET state\n");
	487	+ fdctrl.state &= ~(FD_CTRL_RESET \| FD_CTRL_SLEEP);
	488	+ }
	489	+ }
	490	+ /* Selected drive */
	491	+ fdctrl.cur_drv = value & 1;
	492	+}
	493	+
	494	+/* Tape drive register : 0x03 */
	495	+static uint32_t fdctrl_read_tape (CPUState *env, uint32_t reg)
	496	+{
	497	+ uint32_t retval = 0;
	498	+
	499	+ fdctrl_reset_irq();
	500	+ /* Disk boot selection indicator */
	501	+ retval \|= fdctrl.bootsel << 2;
	502	+ /* Tape indicators: never allowed */
	503	+ FLOPPY_DPRINTF("tape drive register: 0x%02x\n", retval);
	504	+
	505	+ return retval;
	506	+}
	507	+
	508	+static void fdctrl_write_tape (CPUState *env, uint32_t reg, uint32_t value)
	509	+{
	510	+ fdctrl_reset_irq();
	511	+ /* Reset mode */
	512	+ if (fdctrl.state & FD_CTRL_RESET) {
	513	+ FLOPPY_DPRINTF("Floppy controler in RESET state !\n");
	514	+ return;
	515	+ }
	516	+ FLOPPY_DPRINTF("tape drive register set to 0x%02x\n", value);
	517	+ /* Disk boot selection indicator */
	518	+ fdctrl.bootsel = (value >> 2) & 1;
	519	+ /* Tape indicators: never allow */
	520	+}
	521	+
	522	+/* Main status register : 0x04 (read) */
	523	+static uint32_t fdctrl_read_main_status (CPUState *env, uint32_t reg)
	524	+{
	525	+ uint32_t retval = 0;
	526	+
	527	+ fdctrl_reset_irq();
	528	+ fdctrl.state &= ~(FD_CTRL_SLEEP \| FD_CTRL_RESET);
	529	+ if (!(fdctrl.state & FD_CTRL_BUSY)) {
	530	+ /* Data transfer allowed */
	531	+ retval \|= 0x80;
	532	+ /* Data transfer direction indicator */
	533	+ if (fdctrl.data_dir == FD_DIR_READ)
	534	+ retval \|= 0x40;
	535	+ }
	536	+ /* Should handle 0x20 for SPECIFY command */
	537	+ /* Command busy indicator */
	538	+ if (FD_STATE(fdctrl.data_state) == FD_STATE_DATA \|\|
	539	+ FD_STATE(fdctrl.data_state) == FD_STATE_STATUS)
	540	+ retval \|= 0x10;
	541	+ FLOPPY_DPRINTF("main status register: 0x%02x\n", retval);
	542	+
	543	+ return retval;
	544	+}
	545	+
	546	+/* Data select rate register : 0x04 (write) */
	547	+static void fdctrl_write_rate (CPUState *env, uint32_t reg, uint32_t value)
	548	+{
	549	+ fdctrl_reset_irq();
	550	+ /* Reset mode */
	551	+ if (fdctrl.state & FD_CTRL_RESET) {
	552	+ if (reg != 0x2 \|\| !(value & 0x04)) {
	553	+ FLOPPY_DPRINTF("Floppy controler in RESET state !\n");
	554	+ return;
	555	+ }
	556	+ }
	557	+ FLOPPY_DPRINTF("select rate register set to 0x%02x\n", value);
	558	+ /* Reset: autoclear */
	559	+ if (value & 0x80) {
	560	+ fdctrl.state \|= FD_CTRL_RESET;
	561	+ fdctrl_reset(1);
	562	+ fdctrl.state &= ~FD_CTRL_RESET;
	563	+ }
	564	+ if (value & 0x40) {
	565	+ fdctrl.state \|= FD_CTRL_SLEEP;
	566	+ fdctrl_reset(1);
	567	+ }
	568	+// fdctrl.precomp = (value >> 2) & 0x07;
	569	+}
	570	+
	571	+/* Digital input register : 0x07 (read-only) */
	572	+static uint32_t fdctrl_read_dir (CPUState *env, uint32_t reg)
	573	+{
	574	+ fdrive_t drv0, drv1;
	575	+ uint32_t retval = 0;
	576	+
	577	+ fdctrl_reset_irq();
	578	+ drv0 = &fdctrl.drives[fdctrl.bootsel];
	579	+ drv1 = &fdctrl.drives[1 - fdctrl.bootsel];
	580	+ if (drv0->rv \|\| drv1->rv)
	581	+ retval \|= 0x80;
	582	+ if (retval != 0)
	583	+ FLOPPY_ERROR("Floppy digital input register: 0x%02x\n", retval);
	584	+ drv0->rv = 0;
	585	+ drv1->rv = 0;
	586	+
	587	+ return retval;
	588	+}
	589	+
	590	+/* FIFO state control */
	591	+static void fdctrl_reset_fifo (void)
	592	+{
	593	+ fdctrl.data_dir = FD_DIR_WRITE;
	594	+ fdctrl.data_pos = 0;
	595	+ fdctrl.data_state = FD_STATE_CMD;
	596	+}
	597	+
	598	+/* Set FIFO status for the host to read */
	599	+static void fdctrl_set_fifo (int fifo_len, int do_irq)
	600	+{
	601	+ fdctrl.data_dir = FD_DIR_READ;
	602	+ fdctrl.data_len = fifo_len;
	603	+ fdctrl.data_pos = 0;
	604	+ fdctrl.data_state = FD_STATE_STATUS;
	605	+ if (do_irq)
	606	+ fdctrl_raise_irq(0x00);
	607	+}
	608	+
	609	+/* Set an error: unimplemented/unknown command */
	610	+static void fdctrl_unimplemented (void)
	611	+{
	612	+#if 0
	613	+ fdrive_t cur_drv, drv0, *drv1;
	614	+
	615	+ drv0 = &fdctrl.drives[fdctrl.bootsel];
	616	+ drv1 = &fdctrl.drives[1 - fdctrl.bootsel];
	617	+ cur_drv = fdctrl.cur_drv == 0 ? drv0 : drv1;
	618	+ fdctrl.fifo[0] = 0x60 \| (cur_drv->head << 1) \| fdctrl.cur_drv;
	619	+ fdctrl.fifo[1] = 0x00;
	620	+ fdctrl.fifo[2] = 0x00;
	621	+ fdctrl_set_fifo(3, 1);
	622	+#else
	623	+ fdctrl_reset_fifo();
	624	+#endif
	625	+}
	626	+
	627	+/* Callback for transfer end (stop or abort) */
	628	+static void fdctrl_stop_transfer (uint8_t status0, uint8_t status1,
	629	+ uint8_t status2)
	630	+{
	631	+ fdrive_t cur_drv, drv0, *drv1;
	632	+
	633	+ drv0 = &fdctrl.drives[fdctrl.bootsel];
	634	+ drv1 = &fdctrl.drives[1 - fdctrl.bootsel];
	635	+ cur_drv = fdctrl.cur_drv == 0 ? drv0 : drv1;
	636	+ FLOPPY_DPRINTF("transfer status: %02x %02x %02x (%02x)\n",
	637	+ status0, status1, status2,
	638	+ status0 \| (cur_drv->head << 1) \| fdctrl.cur_drv);
	639	+ fdctrl.fifo[0] = status0 \| (cur_drv->head << 1) \| fdctrl.cur_drv;
	640	+ fdctrl.fifo[1] = status1;
	641	+ fdctrl.fifo[2] = status2;
	642	+ fdctrl.fifo[3] = cur_drv->track;
	643	+ fdctrl.fifo[4] = cur_drv->head;
	644	+ fdctrl.fifo[5] = cur_drv->sect;
	645	+ fdctrl.fifo[6] = FD_SECTOR_SC;
	646	+ fdctrl.data_dir = FD_DIR_READ;
	647	+ if (fdctrl.state & FD_CTRL_BUSY)
	648	+ DMA_release_DREQ(fdctrl.dma_chann);
	649	+ fdctrl_set_fifo(7, 1);
	650	+}
	651	+
	652	+/* Prepare a data transfer (either DMA or FIFO) */
	653	+static void fdctrl_start_transfer (int direction)
	654	+{
	655	+ fdrive_t cur_drv, drv0, *drv1;
	656	+ uint8_t kh, kt, ks;
	657	+ int did_seek;
	658	+
	659	+ drv0 = &fdctrl.drives[fdctrl.bootsel];
	660	+ drv1 = &fdctrl.drives[1 - fdctrl.bootsel];
	661	+ fdctrl.cur_drv = fdctrl.fifo[1] & 1;
	662	+ cur_drv = fdctrl.cur_drv == 0 ? drv0 : drv1;
	663	+ kt = fdctrl.fifo[2];
	664	+ kh = fdctrl.fifo[3];
	665	+ ks = fdctrl.fifo[4];
	666	+ FLOPPY_DPRINTF("Start tranfert at %d %d %02x %02x (%d)\n",
	667	+ fdctrl.cur_drv, kh, kt, ks,
	668	+ _fd_sector(kh, kt, ks, cur_drv->last_sect));
	669	+ did_seek = 0;
	670	+ switch (fd_seek(cur_drv, kh, kt, ks, fdctrl.config & 0x40)) {
	671	+ case 2:
	672	+ /* sect too big */
	673	+ fdctrl_stop_transfer(0x40, 0x00, 0x00);
	674	+ fdctrl.fifo[3] = kt;
	675	+ fdctrl.fifo[4] = kh;
	676	+ fdctrl.fifo[5] = ks;
	677	+ return;
	678	+ case 3:
	679	+ /* track too big */
	680	+ fdctrl_stop_transfer(0x40, 0x80, 0x00);
	681	+ fdctrl.fifo[3] = kt;
	682	+ fdctrl.fifo[4] = kh;
	683	+ fdctrl.fifo[5] = ks;
	684	+ return;
	685	+ case 4:
	686	+ /* No seek enabled */
	687	+ fdctrl_stop_transfer(0x40, 0x00, 0x00);
	688	+ fdctrl.fifo[3] = kt;
	689	+ fdctrl.fifo[4] = kh;
	690	+ fdctrl.fifo[5] = ks;
	691	+ return;
	692	+ case 1:
	693	+ did_seek = 1;
	694	+ break;
	695	+ default:
	696	+ break;
	697	+ }
	698	+ /* Set the FIFO state */
	699	+ fdctrl.data_dir = direction;
	700	+ fdctrl.data_pos = 0;
	701	+ fdctrl.data_state = FD_STATE_DATA; /* FIFO ready for data */
	702	+ if (fdctrl.fifo[0] & 0x80)
	703	+ fdctrl.data_state \|= FD_STATE_MULTI;
	704	+ if (did_seek)
	705	+ fdctrl.data_state \|= FD_STATE_SEEK;
	706	+ if (fdctrl.dma_en) {
	707	+ int dma_mode;
	708	+ /* DMA transfer are enabled. Check if DMA channel is well programmed */
	709	+ dma_mode = DMA_get_channel_mode(fdctrl.dma_chann);
	710	+ dma_mode = (dma_mode >> 2) & 3;
	711	+ FLOPPY_DPRINTF("dma_mode=%d direction=%d (%d)\n", dma_mode, direction,
	712	+ (128 << fdctrl.fifo[5]) *
	713	+ (cur_drv->last_sect - ks + 1));
	714	+ if (((direction == FD_DIR_SCANE \|\| direction == FD_DIR_SCANL \|\|
	715	+ direction == FD_DIR_SCANH) && dma_mode == 0) \|\|
	716	+ (direction == FD_DIR_WRITE && dma_mode == 2) \|\|
	717	+ (direction == FD_DIR_READ && dma_mode == 1)) {
	718	+ /* No access is allowed until DMA transfer has completed */
	719	+ fdctrl.state \|= FD_CTRL_BUSY;
	720	+ /* Now, we just have to wait for the DMA controler to
	721	+ * recall us...
	722	+ */
	723	+ DMA_hold_DREQ(fdctrl.dma_chann);
	724	+ return;
	725	+ }
	726	+ }
	727	+ FLOPPY_DPRINTF("start non-DMA transfer\n");
	728	+ /* IO based transfer: calculate len */
	729	+ if (fdctrl.fifo[5] == 00) {
	730	+ fdctrl.data_len = fdctrl.fifo[8];
	731	+ } else {
	732	+ fdctrl.data_len = 128 << fdctrl.fifo[5];
	733	+ fdctrl.data_len *= (cur_drv->last_sect - ks + 1);
	734	+ if (fdctrl.fifo[0] & 0x80)
	735	+ fdctrl.data_len *= 2;
	736	+ }
	737	+ fdctrl_raise_irq(0x00);
	738	+
	739	+ return;
	740	+}
	741	+
	742	+/* Prepare a transfer of deleted data */
	743	+static void fdctrl_start_transfer_del (int direction)
	744	+{
	745	+ /* We don't handle deleted data,
	746	+ * so we don't return ANYTHING
	747	+ */
	748	+ fdctrl_stop_transfer(0x60, 0x00, 0x00);
	749	+}
	750	+
	751	+/* handlers for DMA transfers */
	752	+static int fdctrl_transfer_handler (uint32_t addr, int size, int *irq)
	753	+{
	754	+ fdrive_t cur_drv, drv0, *drv1;
	755	+ void *orig;
	756	+ int len;
	757	+ uint8_t status0 = 0x00, status1 = 0x00, status2 = 0x00;
	758	+
	759	+ fdctrl_reset_irq();
	760	+ if (!(fdctrl.state & FD_CTRL_BUSY)) {
	761	+ FLOPPY_DPRINTF("Not in DMA transfer mode !\n");
	762	+ return 0;
	763	+ }
	764	+ drv0 = &fdctrl.drives[fdctrl.bootsel];
	765	+ drv1 = &fdctrl.drives[1 - fdctrl.bootsel];
	766	+ cur_drv = fdctrl.cur_drv == 0 ? drv0 : drv1;
	767	+// *irq = fdctrl.irq_lvl;
	768	+ *irq = -1;
	769	+ if (fdctrl.data_dir == FD_DIR_SCANE \|\| fdctrl.data_dir == FD_DIR_SCANL \|\|
	770	+ fdctrl.data_dir == FD_DIR_SCANH)
	771	+ status2 = 0x04;
	772	+ for (fdctrl.data_len = size; fdctrl.data_pos < fdctrl.data_len;
	773	+ fdctrl.data_pos += len) {
	774	+ len = size - fdctrl.data_pos;
	775	+ if (len > FD_SECTOR_LEN)
	776	+ len = FD_SECTOR_LEN;
	777	+ FLOPPY_DPRINTF("copy %d bytes (%d %d %d) %d pos %d %02x %02x "
	778	+ "(%d-0x%08x)\n", len, size, fdctrl.data_pos,
	779	+ fdctrl.data_len, fdctrl.cur_drv, cur_drv->head,
	780	+ cur_drv->track, cur_drv->sect, fd_sector(cur_drv),
	781	+ fd_sector(cur_drv) * 512);
	782	+ if (len < FD_SECTOR_LEN) {
	783	+ memset(&fdctrl.fifo[FD_SECTOR_LEN - len], 0,
	784	+ FD_SECTOR_LEN - len - 1);
	785	+ orig = fdctrl.fifo;
	786	+ } else {
	787	+ orig = (void *)(addr + fdctrl.data_pos);
	788	+ }
	789	+ if (fdctrl.data_dir != FD_DIR_WRITE) {
	790	+ /* READ & SCAN commands */
	791	+ if (cur_drv->bs == NULL \|\|
	792	+ bdrv_read(cur_drv->bs, fd_sector(cur_drv), orig, 1) < 0) {
	793	+ FLOPPY_DPRINTF("Floppy: error getting sector %d\n",
	794	+ fd_sector(cur_drv));
	795	+ /* Sure, image size is too small... */
	796	+ memset((void *)(addr + fdctrl.data_pos), 0, FD_SECTOR_LEN);
	797	+ }
	798	+ if (fdctrl.data_dir == FD_DIR_READ) {
	799	+ if (len < FD_SECTOR_LEN) {
	800	+ memcpy((void *)(addr + fdctrl.data_pos),
	801	+ fdctrl.fifo, FD_SECTOR_LEN);
	802	+ }
	803	+ } else {
	804	+ int ret;
	805	+ ret = memcmp((void *)(addr + fdctrl.data_pos),
	806	+ fdctrl.fifo, FD_SECTOR_LEN);
	807	+ if (ret == 0) {
	808	+ status2 = 0x08;
	809	+ goto end_transfer;
	810	+ }
	811	+ if ((ret < 0 && fdctrl.data_dir == FD_DIR_SCANL) \|\|
	812	+ (ret > 0 && fdctrl.data_dir == FD_DIR_SCANH)) {
	813	+ status2 = 0x00;
	814	+ goto end_transfer;
	815	+ }
	816	+ }
	817	+ } else {
	818	+ /* WRITE commands */
	819	+ if (cur_drv->bs == NULL \|\|
	820	+ bdrv_write(cur_drv->bs, fd_sector(cur_drv), orig, 1) < 0) {
	821	+ FLOPPY_ERROR("writting sector %d\n", fd_sector(cur_drv));
	822	+ fdctrl_stop_transfer(0x60, 0x00, 0x00);
	823	+ goto transfer_error;
	824	+ }
	825	+ }
	826	+ if (len == FD_SECTOR_LEN) {
	827	+ /* Seek to next sector */
	828	+ if (cur_drv->sect == cur_drv->last_sect) {
	829	+ if (cur_drv->head == 0) {
	830	+ cur_drv->head = 1;
	831	+ } else {
	832	+ cur_drv->track++;
	833	+ cur_drv->head = 0;
	834	+ }
	835	+ cur_drv->sect = 1;
	836	+ FLOPPY_DPRINTF("seek to next sector (%d %02x %02x => %d)\n",
	837	+ cur_drv->head, cur_drv->track, cur_drv->sect,
	838	+ fd_sector(cur_drv));
	839	+ if (cur_drv->head == 0) {
	840	+ FLOPPY_DPRINTF("end transfer\n");
	841	+ goto end_transfer;
	842	+ }
	843	+ if (!FD_MULTI_TRACK(fdctrl.data_state)) {
	844	+ /* Single track read */
	845	+ FLOPPY_DPRINTF("single track transfert: end transfer\n");
	846	+// status1 \|= 0x80;
	847	+ goto end_transfer;
	848	+ }
	849	+ } else {
	850	+ cur_drv->sect++;
	851	+ FLOPPY_DPRINTF("seek to next sector (%d %02x %02x => %d)\n",
	852	+ cur_drv->head, cur_drv->track, cur_drv->sect,
	853	+ fd_sector(cur_drv));
	854	+ }
	855	+ }
	856	+ }
	857	+end_transfer:
	858	+ if (fdctrl.data_dir == FD_DIR_SCANE \|\|
	859	+ fdctrl.data_dir == FD_DIR_SCANL \|\|
	860	+ fdctrl.data_dir == FD_DIR_SCANH)
	861	+ status2 = 0x08;
	862	+ if (FD_DID_SEEK(fdctrl.data_state))
	863	+ status0 \|= 0x20;
	864	+ fdctrl_stop_transfer(status0, status1, status2);
	865	+transfer_error:
	866	+
	867	+ return fdctrl.data_pos;
	868	+}
	869	+
	870	+/* Unused... */
	871	+static int fdctrl_misc_handler (int duknwo)
	872	+{
	873	+ return -1;
	874	+}
	875	+
	876	+/* Data register : 0x05 */
	877	+static uint32_t fdctrl_read_data (CPUState *env, uint32_t reg)
	878	+{
	879	+ fdrive_t cur_drv, drv0, *drv1;
	880	+ uint32_t retval = 0;
	881	+ int pos, len;
	882	+
	883	+ fdctrl_reset_irq();
	884	+ drv0 = &fdctrl.drives[fdctrl.bootsel];
	885	+ drv1 = &fdctrl.drives[1 - fdctrl.bootsel];
	886	+ cur_drv = fdctrl.cur_drv == 0 ? drv0 : drv1;
	887	+ fdctrl.state &= ~FD_CTRL_SLEEP;
	888	+ if (FD_STATE(fdctrl.data_state) == FD_STATE_CMD) {
	889	+ FLOPPY_ERROR("can't read data in CMD state\n");
	890	+ return 0;
	891	+ }
	892	+ pos = fdctrl.data_pos;
	893	+ if (FD_STATE(fdctrl.data_state) == FD_STATE_DATA) {
	894	+ pos %= FD_SECTOR_LEN;
	895	+ if (pos == 0) {
	896	+ len = fdctrl.data_len - fdctrl.data_pos;
	897	+ if (len > FD_SECTOR_LEN)
	898	+ len = FD_SECTOR_LEN;
	899	+ bdrv_read(cur_drv->bs, fd_sector(cur_drv),
	900	+ fdctrl.fifo, len);
	901	+ }
	902	+ }
	903	+ retval = fdctrl.fifo[pos];
	904	+ if (++fdctrl.data_pos == fdctrl.data_len) {
	905	+ fdctrl.data_pos = 0;
	906	+ /* Switch from transfert mode to status mode
	907	+ * then from status mode to command mode
	908	+ */
	909	+ if (FD_STATE(fdctrl.data_state) == FD_STATE_DATA)
	910	+ fdctrl_stop_transfer(0x20, 0x00, 0x00);
	911	+ else
	912	+ fdctrl_reset_fifo();
	913	+ }
	914	+ FLOPPY_DPRINTF("data register: 0x%02x\n", retval);
	915	+
	916	+ return retval;
	917	+}
	918	+
	919	+static void fdctrl_write_data (CPUState *env, uint32_t reg, uint32_t value)
	920	+{
	921	+ fdrive_t cur_drv, drv0, *drv1;
	922	+
	923	+ fdctrl_reset_irq();
	924	+ drv0 = &fdctrl.drives[fdctrl.bootsel];
	925	+ drv1 = &fdctrl.drives[1 - fdctrl.bootsel];
	926	+ cur_drv = fdctrl.cur_drv == 0 ? drv0 : drv1;
	927	+ /* Reset mode */
	928	+ if (fdctrl.state & FD_CTRL_RESET) {
	929	+ FLOPPY_DPRINTF("Floppy controler in RESET state !\n");
	930	+ return;
	931	+ }
	932	+ fdctrl.state &= ~FD_CTRL_SLEEP;
	933	+ if ((fdctrl.data_state & FD_STATE_STATE) == FD_STATE_STATUS) {
	934	+ FLOPPY_ERROR("can't write data in status mode\n");
	935	+ return;
	936	+ }
	937	+ /* Is it write command time ? */
	938	+ if (FD_STATE(fdctrl.data_state) == FD_STATE_DATA) {
	939	+ /* FIFO data write */
	940	+ fdctrl.fifo[fdctrl.data_pos++] = value;
	941	+ if (fdctrl.data_pos % FD_SECTOR_LEN == (FD_SECTOR_LEN - 1) \|\|
	942	+ fdctrl.data_pos == fdctrl.data_len) {
	943	+ bdrv_write(cur_drv->bs, fd_sector(cur_drv),
	944	+ fdctrl.fifo, FD_SECTOR_LEN);
	945	+ }
	946	+ /* Switch from transfert mode to status mode
	947	+ * then from status mode to command mode
	948	+ */
	949	+ if (FD_STATE(fdctrl.data_state) == FD_STATE_DATA)
	950	+ fdctrl_stop_transfer(0x20, 0x00, 0x00);
	951	+ return;
	952	+ }
	953	+ if (fdctrl.data_pos == 0) {
	954	+ /* Command */
	955	+ switch (value & 0x5F) {
	956	+ case 0x46:
	957	+ /* READ variants */
	958	+ FLOPPY_DPRINTF("READ command\n");
	959	+ /* 8 parameters cmd */
	960	+ fdctrl.data_len = 9;
	961	+ goto enqueue;
	962	+ case 0x4C:
	963	+ /* READ_DELETED variants */
	964	+ FLOPPY_DPRINTF("READ_DELETED command\n");
	965	+ /* 8 parameters cmd */
	966	+ fdctrl.data_len = 9;
	967	+ goto enqueue;
	968	+ case 0x50:
	969	+ /* SCAN_EQUAL variants */
	970	+ FLOPPY_DPRINTF("SCAN_EQUAL command\n");
	971	+ /* 8 parameters cmd */
	972	+ fdctrl.data_len = 9;
	973	+ goto enqueue;
	974	+ case 0x56:
	975	+ /* VERIFY variants */
	976	+ FLOPPY_DPRINTF("VERIFY command\n");
	977	+ /* 8 parameters cmd */
	978	+ fdctrl.data_len = 9;
	979	+ goto enqueue;
	980	+ case 0x59:
	981	+ /* SCAN_LOW_OR_EQUAL variants */
	982	+ FLOPPY_DPRINTF("SCAN_LOW_OR_EQUAL command\n");
	983	+ /* 8 parameters cmd */
	984	+ fdctrl.data_len = 9;
	985	+ goto enqueue;
	986	+ case 0x5D:
	987	+ /* SCAN_HIGH_OR_EQUAL variants */
	988	+ FLOPPY_DPRINTF("SCAN_HIGH_OR_EQUAL command\n");
	989	+ /* 8 parameters cmd */
	990	+ fdctrl.data_len = 9;
	991	+ goto enqueue;
	992	+ default:
	993	+ break;
	994	+ }
	995	+ switch (value & 0x7F) {
	996	+ case 0x45:
	997	+ /* WRITE variants */
	998	+ FLOPPY_DPRINTF("WRITE command\n");
	999	+ /* 8 parameters cmd */
	1000	+ fdctrl.data_len = 9;
	1001	+ goto enqueue;
	1002	+ case 0x49:
	1003	+ /* WRITE_DELETED variants */
	1004	+ FLOPPY_DPRINTF("WRITE_DELETED command\n");
	1005	+ /* 8 parameters cmd */
	1006	+ fdctrl.data_len = 9;
	1007	+ goto enqueue;
	1008	+ default:
	1009	+ break;
	1010	+ }
	1011	+ switch (value) {
	1012	+ case 0x03:
	1013	+ /* SPECIFY */
	1014	+ FLOPPY_DPRINTF("SPECIFY command\n");
	1015	+ /* 1 parameter cmd */
	1016	+ fdctrl.data_len = 3;
	1017	+ goto enqueue;
	1018	+ case 0x04:
	1019	+ /* SENSE_DRIVE_STATUS */
	1020	+ FLOPPY_DPRINTF("SENSE_DRIVE_STATUS command\n");
	1021	+ /* 1 parameter cmd */
	1022	+ fdctrl.data_len = 2;
	1023	+ goto enqueue;
	1024	+ case 0x07:
	1025	+ /* RECALIBRATE */
	1026	+ FLOPPY_DPRINTF("RECALIBRATE command\n");
	1027	+ /* 1 parameter cmd */
	1028	+ fdctrl.data_len = 2;
	1029	+ goto enqueue;
	1030	+ case 0x08:
	1031	+ /* SENSE_INTERRUPT_STATUS */
	1032	+ FLOPPY_DPRINTF("SENSE_INTERRUPT_STATUS command (%02x)\n",
	1033	+ fdctrl.int_status);
	1034	+ /* No parameters cmd: returns status if no interrupt */
	1035	+ fdctrl.fifo[0] =
	1036	+ fdctrl.int_status \| (cur_drv->head << 2) \| fdctrl.cur_drv;
	1037	+ fdctrl.fifo[1] = cur_drv->track;
	1038	+ fdctrl_set_fifo(2, 0);
	1039	+ return;
	1040	+ case 0x0E:
	1041	+ /* DUMPREG */
	1042	+ FLOPPY_DPRINTF("DUMPREG command\n");
	1043	+ /* Drives position */
	1044	+ fdctrl.fifo[0] = drv0->track;
	1045	+ fdctrl.fifo[1] = drv1->track;
	1046	+ fdctrl.fifo[2] = 0;
	1047	+ fdctrl.fifo[3] = 0;
	1048	+ /* timers */
	1049	+ fdctrl.fifo[4] = fdctrl.timer0;
	1050	+ fdctrl.fifo[5] = (fdctrl.timer1 << 1) \| fdctrl.dma_en;
	1051	+ fdctrl.fifo[6] = cur_drv->last_sect;
	1052	+ fdctrl.fifo[7] = (fdctrl.lock << 7) \|
	1053	+ (cur_drv->perpendicular << 2);
	1054	+ fdctrl.fifo[8] = fdctrl.config;
	1055	+ fdctrl.fifo[9] = fdctrl.precomp_trk;
	1056	+ fdctrl_set_fifo(10, 0);
	1057	+ return;
	1058	+ case 0x0F:
	1059	+ /* SEEK */
	1060	+ FLOPPY_DPRINTF("SEEK command\n");
	1061	+ /* 2 parameters cmd */
	1062	+ fdctrl.data_len = 3;
	1063	+ goto enqueue;
	1064	+ case 0x10:
	1065	+ /* VERSION */
	1066	+ FLOPPY_DPRINTF("VERSION command\n");
	1067	+ /* No parameters cmd */
	1068	+ /* Controler's version */
	1069	+ fdctrl.fifo[0] = fdctrl.version;
	1070	+ fdctrl_set_fifo(1, 1);
	1071	+ return;
	1072	+ case 0x12:
	1073	+ /* PERPENDICULAR_MODE */
	1074	+ FLOPPY_DPRINTF("PERPENDICULAR_MODE command\n");
	1075	+ /* 1 parameter cmd */
	1076	+ fdctrl.data_len = 2;
	1077	+ goto enqueue;
	1078	+ case 0x13:
	1079	+ /* CONFIGURE */
	1080	+ FLOPPY_DPRINTF("CONFIGURE command\n");
	1081	+ /* 3 parameters cmd */
	1082	+ fdctrl.data_len = 4;
	1083	+ goto enqueue;
	1084	+ case 0x14:
	1085	+ /* UNLOCK */
	1086	+ FLOPPY_DPRINTF("UNLOCK command\n");
	1087	+ /* No parameters cmd */
	1088	+ fdctrl.lock = 0;
	1089	+ fdctrl.fifo[0] = 0;
	1090	+ fdctrl_set_fifo(1, 0);
	1091	+ return;
	1092	+ case 0x17:
	1093	+ /* POWERDOWN_MODE */
	1094	+ FLOPPY_DPRINTF("POWERDOWN_MODE command\n");
	1095	+ /* 2 parameters cmd */
	1096	+ fdctrl.data_len = 3;
	1097	+ goto enqueue;
	1098	+ case 0x18:
	1099	+ /* PART_ID */
	1100	+ FLOPPY_DPRINTF("PART_ID command\n");
	1101	+ /* No parameters cmd */
	1102	+ fdctrl.fifo[0] = 0x41; /* Stepping 1 */
	1103	+ fdctrl_set_fifo(1, 0);
	1104	+ return;
	1105	+ case 0x2C:
	1106	+ /* SAVE */
	1107	+ FLOPPY_DPRINTF("SAVE command\n");
	1108	+ /* No parameters cmd */
	1109	+ fdctrl.fifo[0] = 0;
	1110	+ fdctrl.fifo[1] = 0;
	1111	+ /* Drives position */
	1112	+ fdctrl.fifo[2] = drv0->track;
	1113	+ fdctrl.fifo[3] = drv1->track;
	1114	+ fdctrl.fifo[4] = 0;
	1115	+ fdctrl.fifo[5] = 0;
	1116	+ /* timers */
	1117	+ fdctrl.fifo[6] = fdctrl.timer0;
	1118	+ fdctrl.fifo[7] = fdctrl.timer1;
	1119	+ fdctrl.fifo[8] = cur_drv->last_sect;
	1120	+ fdctrl.fifo[9] = (fdctrl.lock << 7) \|
	1121	+ (cur_drv->perpendicular << 2);
	1122	+ fdctrl.fifo[10] = fdctrl.config;
	1123	+ fdctrl.fifo[11] = fdctrl.precomp_trk;
	1124	+ fdctrl.fifo[12] = fdctrl.pwrd;
	1125	+ fdctrl.fifo[13] = 0;
	1126	+ fdctrl.fifo[14] = 0;
	1127	+ fdctrl_set_fifo(15, 1);
	1128	+ return;
	1129	+ case 0x33:
	1130	+ /* OPTION */
	1131	+ FLOPPY_DPRINTF("OPTION command\n");
	1132	+ /* 1 parameter cmd */
	1133	+ fdctrl.data_len = 2;
	1134	+ goto enqueue;
	1135	+ case 0x42:
	1136	+ /* READ_TRACK */
	1137	+ FLOPPY_DPRINTF("READ_TRACK command\n");
	1138	+ /* 8 parameters cmd */
	1139	+ fdctrl.data_len = 9;
	1140	+ goto enqueue;
	1141	+ case 0x4A:
	1142	+ /* READ_ID */
	1143	+ FLOPPY_DPRINTF("READ_ID command\n");
	1144	+ /* 1 parameter cmd */
	1145	+ fdctrl.data_len = 2;
	1146	+ goto enqueue;
	1147	+ case 0x4C:
	1148	+ /* RESTORE */
	1149	+ FLOPPY_DPRINTF("RESTORE command\n");
	1150	+ /* 17 parameters cmd */
	1151	+ fdctrl.data_len = 18;
	1152	+ goto enqueue;
	1153	+ case 0x4D:
	1154	+ /* FORMAT_TRACK */
	1155	+ FLOPPY_DPRINTF("FORMAT_TRACK command\n");
	1156	+ /* 5 parameters cmd */
	1157	+ fdctrl.data_len = 9;
	1158	+ goto enqueue;
	1159	+ case 0x8E:
	1160	+ /* DRIVE_SPECIFICATION_COMMAND */
	1161	+ FLOPPY_DPRINTF("DRIVE_SPECIFICATION_COMMAND command\n");
	1162	+ /* 5 parameters cmd */
	1163	+ fdctrl.data_len = 6;
	1164	+ goto enqueue;
	1165	+ case 0x8F:
	1166	+ /* RELATIVE_SEEK_OUT */
	1167	+ FLOPPY_DPRINTF("RELATIVE_SEEK_OUT command\n");
	1168	+ /* 2 parameters cmd */
	1169	+ fdctrl.data_len = 3;
	1170	+ goto enqueue;
	1171	+ case 0x94:
	1172	+ /* LOCK */
	1173	+ FLOPPY_DPRINTF("LOCK command\n");
	1174	+ /* No parameters cmd */
	1175	+ fdctrl.lock = 1;
	1176	+ fdctrl.fifo[0] = 0x10;
	1177	+ fdctrl_set_fifo(1, 1);
	1178	+ return;
	1179	+ case 0xCD:
	1180	+ /* FORMAT_AND_WRITE */
	1181	+ FLOPPY_DPRINTF("FORMAT_AND_WRITE command\n");
	1182	+ /* 10 parameters cmd */
	1183	+ fdctrl.data_len = 11;
	1184	+ goto enqueue;
	1185	+ case 0xCF:
	1186	+ /* RELATIVE_SEEK_IN */
	1187	+ FLOPPY_DPRINTF("RELATIVE_SEEK_IN command\n");
	1188	+ /* 2 parameters cmd */
	1189	+ fdctrl.data_len = 3;
	1190	+ goto enqueue;
	1191	+ default:
	1192	+ /* Unknown command */
	1193	+ FLOPPY_ERROR("unknown command: 0x%02x\n", value);
	1194	+ fdctrl_unimplemented();
	1195	+ return;
	1196	+ }
	1197	+ }
	1198	+enqueue:
	1199	+ fdctrl.fifo[fdctrl.data_pos] = value;
	1200	+ if (++fdctrl.data_pos == fdctrl.data_len) {
	1201	+ /* We now have all parameters
	1202	+ * and will be able to treat the command
	1203	+ */
	1204	+ switch (fdctrl.fifo[0] & 0x1F) {
	1205	+ case 0x06:
	1206	+ {
	1207	+ /* READ variants */
	1208	+ FLOPPY_DPRINTF("treat READ command\n");
	1209	+ fdctrl_start_transfer(FD_DIR_READ);
	1210	+ return;
	1211	+ }
	1212	+ case 0x0C:
	1213	+ /* READ_DELETED variants */
	1214	+// FLOPPY_DPRINTF("treat READ_DELETED command\n");
	1215	+ FLOPPY_ERROR("treat READ_DELETED command\n");
	1216	+ fdctrl_start_transfer_del(1);
	1217	+ return;
	1218	+ case 0x16:
	1219	+ /* VERIFY variants */
	1220	+// FLOPPY_DPRINTF("treat VERIFY command\n");
	1221	+ FLOPPY_ERROR("treat VERIFY command\n");
	1222	+ fdctrl_stop_transfer(0x20, 0x00, 0x00);
	1223	+ return;
	1224	+ case 0x10:
	1225	+ /* SCAN_EQUAL variants */
	1226	+// FLOPPY_DPRINTF("treat SCAN_EQUAL command\n");
	1227	+ FLOPPY_ERROR("treat SCAN_EQUAL command\n");
	1228	+ fdctrl_start_transfer(FD_DIR_SCANE);
	1229	+ return;
	1230	+ case 0x19:
	1231	+ /* SCAN_LOW_OR_EQUAL variants */
	1232	+// FLOPPY_DPRINTF("treat SCAN_LOW_OR_EQUAL command\n");
	1233	+ FLOPPY_ERROR("treat SCAN_LOW_OR_EQUAL command\n");
	1234	+ fdctrl_start_transfer(FD_DIR_SCANL);
	1235	+ return;
	1236	+ case 0x1D:
	1237	+ /* SCAN_HIGH_OR_EQUAL variants */
	1238	+// FLOPPY_DPRINTF("treat SCAN_HIGH_OR_EQUAL command\n");
	1239	+ FLOPPY_ERROR("treat SCAN_HIGH_OR_EQUAL command\n");
	1240	+ fdctrl_start_transfer(FD_DIR_SCANH);
	1241	+ return;
	1242	+ default:
	1243	+ break;
	1244	+ }
	1245	+ switch (fdctrl.fifo[0] & 0x3F) {
	1246	+ case 0x05:
	1247	+ /* WRITE variants */
	1248	+ FLOPPY_DPRINTF("treat WRITE command (%02x)\n", fdctrl.fifo[0]);
	1249	+ fdctrl_start_transfer(FD_DIR_WRITE);
	1250	+ return;
	1251	+ case 0x09:
	1252	+ /* WRITE_DELETED variants */
	1253	+// FLOPPY_DPRINTF("treat WRITE_DELETED command\n");
	1254	+ FLOPPY_ERROR("treat WRITE_DELETED command\n");
	1255	+ fdctrl_start_transfer_del(FD_DIR_WRITE);
	1256	+ return;
	1257	+ default:
	1258	+ break;
	1259	+ }
	1260	+ switch (fdctrl.fifo[0]) {
	1261	+ case 0x03:
	1262	+ /* SPECIFY */
	1263	+ FLOPPY_DPRINTF("treat SPECIFY command\n");
	1264	+ fdctrl.timer0 = (fdctrl.fifo[1] >> 4) & 0xF;
	1265	+ fdctrl.timer1 = fdctrl.fifo[1] >> 1;
	1266	+ /* No result back */
	1267	+ fdctrl_reset_fifo();
	1268	+ break;
	1269	+ case 0x04:
	1270	+ /* SENSE_DRIVE_STATUS */
	1271	+ FLOPPY_DPRINTF("treat SENSE_DRIVE_STATUS command\n");
	1272	+ fdctrl.cur_drv = fdctrl.fifo[1] & 1;
	1273	+ cur_drv = fdctrl.cur_drv == 0 ? drv0 : drv1;
	1274	+ cur_drv->head = (fdctrl.fifo[1] >> 2) & 1;
	1275	+ /* 1 Byte status back */
	1276	+ fdctrl.fifo[0] = (cur_drv->ro << 6) \|
	1277	+ (cur_drv->track == 0 ? 0x10 : 0x00) \|
	1278	+ fdctrl.cur_drv;
	1279	+ fdctrl_set_fifo(1, 0);
	1280	+ break;
	1281	+ case 0x07:
	1282	+ /* RECALIBRATE */
	1283	+ FLOPPY_DPRINTF("treat RECALIBRATE command\n");
	1284	+ fdctrl.cur_drv = fdctrl.fifo[1] & 1;
	1285	+ cur_drv = fdctrl.cur_drv == 0 ? drv0 : drv1;
	1286	+ fd_recalibrate(cur_drv);
	1287	+ fdctrl_reset_fifo();
	1288	+ /* Raise Interrupt */
	1289	+ fdctrl_raise_irq(0x20);
	1290	+ break;
	1291	+ case 0x0F:
	1292	+ /* SEEK */
	1293	+ FLOPPY_DPRINTF("treat SEEK command\n");
	1294	+ fdctrl.cur_drv = fdctrl.fifo[1] & 1;
	1295	+ cur_drv = fdctrl.cur_drv == 0 ? drv0 : drv1;
	1296	+ if (fdctrl.fifo[2] <= cur_drv->track)
	1297	+ cur_drv->dir = 1;
	1298	+ else
	1299	+ cur_drv->dir = 0;
	1300	+ cur_drv->head = (fdctrl.fifo[1] >> 2) & 1;
	1301	+ if (fdctrl.fifo[2] > cur_drv->max_track) {
	1302	+ fdctrl_raise_irq(0x60);
	1303	+ } else {
	1304	+ cur_drv->track = fdctrl.fifo[2];
	1305	+ fdctrl_reset_fifo();
	1306	+ /* Raise Interrupt */
	1307	+ fdctrl_raise_irq(0x20);
	1308	+ }
	1309	+ break;
	1310	+ case 0x12:
	1311	+ /* PERPENDICULAR_MODE */
	1312	+ FLOPPY_DPRINTF("treat PERPENDICULAR_MODE command\n");
	1313	+ if (fdctrl.fifo[1] & 0x80)
	1314	+ cur_drv->perpendicular = fdctrl.fifo[1] & 0x7;
	1315	+ /* No result back */
	1316	+ fdctrl_reset_fifo();
	1317	+ break;
	1318	+ case 0x13:
	1319	+ /* CONFIGURE */
	1320	+ FLOPPY_DPRINTF("treat CONFIGURE command\n");
	1321	+ fdctrl.config = fdctrl.fifo[2];
	1322	+ fdctrl.precomp_trk = fdctrl.fifo[3];
	1323	+ /* No result back */
	1324	+ fdctrl_reset_fifo();
	1325	+ break;
	1326	+ case 0x17:
	1327	+ /* POWERDOWN_MODE */
	1328	+ FLOPPY_DPRINTF("treat POWERDOWN_MODE command\n");
	1329	+ fdctrl.pwrd = fdctrl.fifo[1];
	1330	+ fdctrl.fifo[0] = fdctrl.fifo[1];
	1331	+ fdctrl_set_fifo(1, 1);
	1332	+ break;
	1333	+ case 0x33:
	1334	+ /* OPTION */
	1335	+ FLOPPY_DPRINTF("treat OPTION command\n");
	1336	+ /* No result back */
	1337	+ fdctrl_reset_fifo();
	1338	+ break;
	1339	+ case 0x42:
	1340	+ /* READ_TRACK */
	1341	+// FLOPPY_DPRINTF("treat READ_TRACK command\n");
	1342	+ FLOPPY_ERROR("treat READ_TRACK command\n");
	1343	+ fdctrl_unimplemented();
	1344	+ break;
	1345	+ case 0x4A:
	1346	+ /* READ_ID */
	1347	+// FLOPPY_DPRINTF("treat READ_ID command\n");
	1348	+ FLOPPY_ERROR("treat READ_ID command\n");
	1349	+ fdctrl_stop_transfer(0x00, 0x00, 0x00);
	1350	+ break;
	1351	+ case 0x4C:
	1352	+ /* RESTORE */
	1353	+ FLOPPY_DPRINTF("treat RESTORE command\n");
	1354	+ /* Drives position */
	1355	+ drv0->track = fdctrl.fifo[3];
	1356	+ drv1->track = fdctrl.fifo[4];
	1357	+ /* timers */
	1358	+ fdctrl.timer0 = fdctrl.fifo[7];
	1359	+ fdctrl.timer1 = fdctrl.fifo[8];
	1360	+ cur_drv->last_sect = fdctrl.fifo[9];
	1361	+ fdctrl.lock = fdctrl.fifo[10] >> 7;
	1362	+ cur_drv->perpendicular = (fdctrl.fifo[10] >> 2) & 0xF;
	1363	+ fdctrl.config = fdctrl.fifo[11];
	1364	+ fdctrl.precomp_trk = fdctrl.fifo[12];
	1365	+ fdctrl.pwrd = fdctrl.fifo[13];
	1366	+ fdctrl_reset_fifo();
	1367	+ break;
	1368	+ case 0x4D:
	1369	+ /* FORMAT_TRACK */
	1370	+// FLOPPY_DPRINTF("treat FORMAT_TRACK command\n");
	1371	+ FLOPPY_ERROR("treat FORMAT_TRACK command\n");
	1372	+ fdctrl_unimplemented();
	1373	+ break;
	1374	+ case 0x8E:
	1375	+ /* DRIVE_SPECIFICATION_COMMAND */
	1376	+ FLOPPY_DPRINTF("treat DRIVE_SPECIFICATION_COMMAND command\n");
	1377	+ if (fdctrl.fifo[fdctrl.data_pos - 1] & 0x80) {
	1378	+ /* Command parameters done */
	1379	+ if (fdctrl.fifo[fdctrl.data_pos - 1] & 0x40) {
	1380	+ fdctrl.fifo[0] = fdctrl.fifo[1];
	1381	+ fdctrl.fifo[2] = 0;
	1382	+ fdctrl.fifo[3] = 0;
	1383	+ fdctrl_set_fifo(4, 1);
	1384	+ } else {
	1385	+ fdctrl_reset_fifo();
	1386	+ }
	1387	+ } else if (fdctrl.data_len > 7) {
	1388	+ /* ERROR */
	1389	+ fdctrl.fifo[0] = 0x80 \|
	1390	+ (cur_drv->head << 2) \| fdctrl.cur_drv;
	1391	+ fdctrl_set_fifo(1, 1);
	1392	+ }
	1393	+ break;
	1394	+ case 0x8F:
	1395	+ /* RELATIVE_SEEK_OUT */
	1396	+ FLOPPY_DPRINTF("treat RELATIVE_SEEK_OUT command\n");
	1397	+ fdctrl.cur_drv = fdctrl.fifo[1] & 1;
	1398	+ cur_drv = fdctrl.cur_drv == 0 ? drv0 : drv1;
	1399	+ cur_drv->head = (fdctrl.fifo[1] >> 2) & 1;
	1400	+ if (fdctrl.fifo[2] + cur_drv->track > cur_drv->max_track) {
	1401	+ /* ERROR */
	1402	+ fdctrl_raise_irq(0x70);
	1403	+ } else {
	1404	+ cur_drv->track += fdctrl.fifo[2];
	1405	+ cur_drv->dir = 0;
	1406	+ fdctrl_reset_fifo();
	1407	+ fdctrl_raise_irq(0x20);
	1408	+ }
	1409	+ break;
	1410	+ case 0xCD:
	1411	+ /* FORMAT_AND_WRITE */
	1412	+// FLOPPY_DPRINTF("treat FORMAT_AND_WRITE command\n");
	1413	+ FLOPPY_ERROR("treat FORMAT_AND_WRITE command\n");
	1414	+ fdctrl_unimplemented();
	1415	+ break;
	1416	+ case 0xCF:
	1417	+ /* RELATIVE_SEEK_IN */
	1418	+ FLOPPY_DPRINTF("treat RELATIVE_SEEK_IN command\n");
	1419	+ fdctrl.cur_drv = fdctrl.fifo[1] & 1;
	1420	+ cur_drv = fdctrl.cur_drv == 0 ? drv0 : drv1;
	1421	+ cur_drv->head = (fdctrl.fifo[1] >> 2) & 1;
	1422	+ if (fdctrl.fifo[2] > cur_drv->track) {
	1423	+ /* ERROR */
	1424	+ fdctrl_raise_irq(0x60);
	1425	+ } else {
	1426	+ fdctrl_reset_fifo();
	1427	+ cur_drv->track -= fdctrl.fifo[2];
	1428	+ cur_drv->dir = 1;
	1429	+ /* Raise Interrupt */
	1430	+ fdctrl_raise_irq(0x20);
	1431	+ }
	1432	+ break;
	1433	+ }
	1434	+ }
	1435	+}
...	...

gwj / at91sam9263 · Commits

GitLab

Floppy disk emulation (Jocelyn Mayer)