From mboxrd@z Thu Jan 1 00:00:00 1970 Received: from list by monty-python.gnu.org with tmda-scanned (Exim 4.24) id 1AKNi7-0002gL-N8 for qemu-devel@nongnu.org; Thu, 13 Nov 2003 15:04:59 -0500 Received: from mail by monty-python.gnu.org with spam-scanned (Exim 4.24) id 1AKNhb-0002bk-JB for qemu-devel@nongnu.org; Thu, 13 Nov 2003 15:04:58 -0500 Received: from [62.210.158.45] (helo=quito.magic.fr) by monty-python.gnu.org with esmtp (Exim 4.24) id 1AKNgn-00021W-JI for qemu-devel@nongnu.org; Thu, 13 Nov 2003 15:03:37 -0500 Received: from 10.0.0.2 (ppp-181.net-555.magic.fr [62.210.255.181]) by quito.magic.fr (8.11.6/8.11.2) with ESMTP id hADJ23v27866 for ; Thu, 13 Nov 2003 20:02:04 +0100 (CET) From: "J. Mayer" Content-Type: text/plain Message-Id: <1068750390.1697.211.camel@rapid> Mime-Version: 1.0 Date: 13 Nov 2003 20:06:30 +0100 Content-Transfer-Encoding: 7bit Subject: [Qemu-devel] [ADD] Floppy disk controler emulation Reply-To: qemu-devel@nongnu.org List-Id: List-Unsubscribe: , List-Archive: List-Post: List-Help: List-Subscribe: , To: qemu-devel@nongnu.org Hello, here's an emulation for Intel 82078 floopy disk controler. It seems to have some bugs (make BSD kernel crash at probe time), but allow Linux kernel boot from a floppy disk: I saw Mandrake 9 booting, as well as OpenBSD 3.2, NetBSD 1.6 & BeOS 5.0. QNX 6.2 loads but doesn't seem to start and RedHat Linux 9.0 makes qemu crash (not related with fd emulation). Please add this file: fdc.c: #include #include #include #include #include "vl.h" uint32_t __attribute((regparm(1))) __ldl_cmmu(unsigned long addr, int is_user); void __attribute((regparm(2))) __stl_cmmu(unsigned long addr, uint32_t v, int is_user); uint8_t __attribute((regparm(1))) __ldb_cmmu(unsigned long addr, int is_user); void __attribute((regparm(2))) __stb_cmmu(unsigned long addr, uint8_t v, int is_user); /********************************************************/ /* debug Floppy devices */ //#define DEBUG_FLOPPY /* 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 */ FDRIVE_DISK_NONE = 0x04, /* */ } 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; typedef struct fdrive_t { BlockDriverState *bs; /* Drive status */ fdrive_type_t drive; uint8_t motor; /* on/off */ uint8_t perpendicular; /* 2.88 MB access mode */ uint8_t rv; /* Revalidated */ /* Position */ uint8_t head; uint8_t track; uint8_t sect; /* Last operation status */ uint8_t dir; /* Direction */ uint8_t rw; /* Read/write */ /* Media */ fdisk_type_t disk; /* Disk type */ uint8_t last_sect; /* Nb sector per track */ uint8_t max_track; /* Nb of tracks */ uint32_t abs_last_sect; /* Size of the disk image */ uint8_t ro; /* Is read-only */ } fdrive_t; /* Not a lot of things to do to init a drive... */ static void fd_init (fdrive_t *drv) { drv->bs = NULL; drv->drive = FDRIVE_DISK_288; drv->perpendicular = 0; } /* Returns current position, in sectors, for given drive */ static int fd_sector (fdrive_t *drv) { return (((drv->track * 2) + drv->head) * drv->last_sect) + drv->sect - 1; } static int fd_seek (fdrive_t *drv, uint8_t head, uint8_t track, uint8_t sect) { uint32_t sector; sector = (((track * 2) + head) * drv->last_sect) + sect - 1; if (sector > drv->abs_last_sect) { printf("FDC error: sector: %d max sect: %d\n", sector, drv->abs_last_sect); return -1; } drv->head = head; drv->track = track; drv->sect = sect; return 0; } /* Set drive back to track 0 */ static void fd_recalibrate (fdrive_t *drv) { #ifdef DEBUG_FLOPPY printf("%s\n", __func__); #endif drv->head = 0; drv->track = 0; drv->sect = 1; drv->dir = 1; drv->rw = 0; } /* Revalidate a disk drive after a disk change */ static void fd_revalidate (fdrive_t *drv, int ro) { int64_t nb_sectors; if (drv->bs != NULL) { bdrv_get_geometry(drv->bs, &nb_sectors); if (nb_sectors > 2880) { /* Pretend we have a 2.88 MB disk */ drv->disk = FDRIVE_DISK_288; drv->last_sect = 36; #if 0 } else if (nb_sectors > 1440) { /* Pretend we have a 1.44 MB disk */ drv->disk = FDRIVE_DISK_144; drv->last_sect = 18; drv->max_track = 80; } else { /* Pretend we have a 720 kB disk */ drv->disk = FDRIVE_DISK_720; drv->last_sect = 9; drv->max_track = 80; } #else } else { /* Pretend we have a 1.44 MB disk */ drv->disk = FDRIVE_DISK_144; drv->last_sect = 18; drv->max_track = 80; } #endif drv->abs_last_sect = nb_sectors; } else { drv->disk = FDRIVE_DISK_NONE; drv->last_sect = 1; /* Avoid eventual divide by 0 bugs */ drv->abs_last_sect = 0; } drv->ro = ro; drv->rv = 1; } /* Motor control */ static void fd_start (fdrive_t *drv) { drv->motor = 1; } static void fd_stop (fdrive_t *drv) { drv->motor = 0; } /* Re-initialise a drives (motor off, repositioned) */ static void fd_reset (fdrive_t *drv) { fd_stop(drv); fd_revalidate(drv, 1); fd_recalibrate(drv); } /********************************************************/ /* Intel 82078 floppy disk controler emulation */ enum { FD_CTRL_ACTIVE = 0x01, FD_CTRL_RESET = 0x02, FD_CTRL_SLEEP = 0x04, FD_CTRL_BUSY = 0x08, }; enum { FD_DIR_WRITE = 0, FD_DIR_READ = 1, FD_DIR_VERIFY = 2, }; enum { FD_STATE_CMD = 0, FD_STATE_STATUS = 1, FD_STATE_DATA = 2, }; #define FD_FIFO_LEN FD_SECTOR_LEN typedef struct fdctrl_t { /* Controler's identification */ uint8_t version; /* HW */ int irq_lvl; int dma_chann; /* Controler state */ uint8_t state; uint8_t dma_en; uint8_t cur_drv; uint8_t bootsel; /* Command FIFO */ uint8_t fifo[FD_FIFO_LEN]; uint32_t data_pos; uint32_t data_len; uint8_t data_state; uint8_t data_dir; /* 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]; } fdctrl_t; static fdctrl_t fdctrl; static void fdctrl_reset (void); static int fdctrl_transfer_handler (uint32_t addr, int size, int *irq); static int fdctrl_misc_handler (int duknwo); static void fdctrl_stop_transfer (uint8_t status0, uint8_t status1); void fdctrl_init (int irq_lvl, int dma_chann) { int i; #ifdef DEBUG_FLOPPY printf("%s\n", __func__); #endif memset(&fdctrl, 0, sizeof(fdctrl)); fdctrl.version = 0x90; /* Intel 82078 controler */ fdctrl.irq_lvl = irq_lvl; fdctrl.dma_chann = dma_chann; fdctrl.config = 0x40; /* Implicit seek, polling & FIFO enabled */ if (fdctrl.dma_chann != -1) { fdctrl.dma_en = 1; DMA_register_channel(dma_chann, &fdctrl_transfer_handler, &fdctrl_misc_handler); } else { fdctrl.dma_en = 0; } for (i = 0; i < MAX_FD; i++) fd_init(&fdctrl.drives[i]); fdctrl_reset(); fdctrl.state = FD_CTRL_ACTIVE; } static void fdctrl_reset (void) { int i; #ifdef DEBUG_FLOPPY printf("%s\n", __func__); #endif /* Initialise controler */ fdctrl.cur_drv = 0; fdctrl.bootsel = 0; /* FDA & FDB aren't interverted by default */ /* FIFO state */ fdctrl.data_pos = 0; fdctrl.data_len = 0; fdctrl.data_state = FD_STATE_CMD; fdctrl.data_dir = FD_DIR_WRITE; for (i = 0; i < MAX_FD; i++) fd_reset(&fdctrl.drives[i]); } /* floppy controler helpers */ /* Reset FIFO state so the host can send commands */ static void fdctrl_reset_state (void) { fdctrl.data_dir = FD_DIR_WRITE; fdctrl.data_pos = 0; fdctrl.data_state = FD_STATE_CMD; } /* Set FIFO status for the host to read */ static void fdctrl_set_state (int fifo_len) { fdctrl.data_dir = FD_DIR_READ; fdctrl.data_len = fifo_len; fdctrl.data_pos = 0; fdctrl.data_state = FD_STATE_STATUS; } /* Set an error: unimplemented/unknown command */ static void fdctrl_unimplemented (void) { fdrive_t *cur_drv, *drv0, *drv1; drv0 = &fdctrl.drives[0]; drv1 = &fdctrl.drives[1]; cur_drv = fdctrl.cur_drv == 0 ? drv0 : drv1; fdctrl.fifo[0] = 0x80 | (cur_drv->head << 2) | fdctrl.cur_drv; fdctrl_set_state(1); } int fdctrl_transfer_handler (uint32_t addr, int size, int *irq) { fdrive_t *cur_drv, *drv0, *drv1; int len, tmp; if (!(fdctrl.state & FD_CTRL_BUSY)) { #ifdef DEBUG_FLOPPY printf("Not in DMA transfer mode !\n"); #endif return 0; } drv0 = &fdctrl.drives[0]; drv1 = &fdctrl.drives[1]; cur_drv = fdctrl.cur_drv == 0 ? drv0 : drv1; *irq = fdctrl.irq_lvl; *irq = -1; for (fdctrl.data_len = size; fdctrl.data_pos < fdctrl.data_len; fdctrl.data_pos += len) { len = size - fdctrl.data_pos; if (len > FD_FIFO_LEN) len = FD_FIFO_LEN; #ifdef DEBUG_FLOPPY printf("Read %d bytes (%d %d %d) from sect %d (0x%08x)\n", len, size, fdctrl.data_pos, fdctrl.data_len, fd_sector(cur_drv), fd_sector(cur_drv) * 512); #endif if (len < FD_FIFO_LEN) memset(&fdctrl.fifo[FD_FIFO_LEN - len], 0, FD_FIFO_LEN - len - 1); if (fdctrl.data_dir != FD_DIR_WRITE) { /* READ & VERIFY commands */ if (bdrv_read(cur_drv->bs, fd_sector(cur_drv), fdctrl.fifo, 1) < 0) { #ifdef DEBUG_FLOPPY printf("Floppy: error getting sector %d\n", fd_sector(cur_drv)); #endif /* Sure, image size is too small... */ memset(fdctrl.fifo, 0, FD_FIFO_LEN); } if (fdctrl.data_dir == FD_DIR_READ) { memcpy((void *)(addr + fdctrl.data_pos), fdctrl.fifo, FD_FIFO_LEN); } else { if (memcmp((void *)(addr + fdctrl.data_pos), fdctrl.fifo, FD_FIFO_LEN) != 0) { /* TODO: set status */ } } } else { /* WRITE commands */ memcpy(fdctrl.fifo, (void *)(addr + fdctrl.data_pos), FD_FIFO_LEN); if (bdrv_write(cur_drv->bs, fd_sector(cur_drv), fdctrl.fifo, 1) < 0) { printf("Floppy: error writting sector %d\n", fd_sector(cur_drv)); fdctrl_stop_transfer(0x60, 0x00); goto transfer_error; } } for (tmp = len; tmp >= FD_SECTOR_LEN; tmp -= FD_SECTOR_LEN) { /* Seek to next sector */ if (cur_drv->sect == cur_drv->last_sect) { if (!(fdctrl.fifo[0] & 0x80)) { /* Single track read */ #ifdef DEBUG_FLOPPY printf("single track read: end transfer\n"); #endif goto end_transfer; } if (cur_drv->head == 0) { cur_drv->head = 1; } else { cur_drv->track++; cur_drv->head = 0; } cur_drv->sect = 0; } else { cur_drv->sect++; } } } end_transfer: fdctrl_stop_transfer(0x20, 0x00); transfer_error: return fdctrl.data_pos; } /* Unused... */ static int fdctrl_misc_handler (int duknwo) { return -1; } static void fdctrl_stop_transfer (uint8_t status0, uint8_t status1) { fdrive_t *cur_drv, *drv0, *drv1; #ifdef DEBUG_FLOPPY printf("%s\n", __func__); #endif drv0 = &fdctrl.drives[0]; drv1 = &fdctrl.drives[1]; cur_drv = fdctrl.cur_drv == 0 ? drv0 : drv1; fdctrl.fifo[0] = status0 | (cur_drv->head << 1) | fdctrl.cur_drv; fdctrl.fifo[1] = status1; /* Status indicates hw error that won't occur here */ fdctrl.fifo[2] = 0x00; 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_set_state(7); if (fdctrl.state & FD_CTRL_BUSY) { DMA_release_DREQ(fdctrl.dma_chann); fdctrl.state &= ~FD_CTRL_BUSY; } #if 1 /* WARNING: * Should be done by the interrupt handler. * have to check what's going wrong... */ __stb_cmmu(0x043E, __ldb_cmmu(0x043E, 0) | 0x80, 0); #endif } static void fdctrl_start_transfer (int direction) { fdrive_t *cur_drv, *drv0, *drv1; uint8_t kh, kt, ks; #ifdef DEBUG_FLOPPY // printf("%s\n", __func__); #endif drv0 = &fdctrl.drives[0]; drv1 = &fdctrl.drives[1]; fdctrl.cur_drv = fdctrl.fifo[1] & 1; cur_drv = fdctrl.cur_drv == 0 ? drv0 : drv1; kt = fdctrl.fifo[2]; kh = fdctrl.fifo[3]; ks = fdctrl.fifo[4]; #if 1 if (fdctrl.fifo[4] > cur_drv->last_sect || /* ((fdctrl.config & 0x40) == 0 && cur_drv->track != fdctrl.fifo[2]) || */ fd_seek(cur_drv, kh, kt, ks) < 0) { /* No implicit seek enabled => ERROR */ #ifdef DEBUG_FLOPPY printf("%s: no implicit SEEK enabled...\n", __func__); printf("asked: %d last_sect=%d\n", fdctrl.fifo[4], cur_drv->last_sect); #endif fdctrl_stop_transfer(0x60, 0x00); fdctrl.fifo[3] = kt; fdctrl.fifo[4] = kh; fdctrl.fifo[5] = ks; } #endif /* Set the FIFO state */ fdctrl.data_dir = direction; fdctrl.data_pos = 0; fdctrl.data_state = FD_STATE_DATA; /* FIFO ready for data */ if (fdctrl.dma_en) { int dma_mode; /* DMA transfer are enabled. Check if DMA channel is well programmed */ dma_mode = DMA_get_channel_mode(fdctrl.dma_chann); dma_mode = (dma_mode >> 2) & 3; #ifdef DEBUG_FLOPPY printf("dma_mode=%d direction=%d\n", dma_mode, direction); #endif if ((direction == FD_DIR_VERIFY && 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 */ fdctrl.state |= FD_CTRL_BUSY; /* Now, we just have to wait for the DMA controler to * recall us... */ DMA_hold_DREQ(fdctrl.dma_chann); return; } } #ifdef DEBUG_FLOPPY printf("%s: non-DMA transfer\n", __func__); #endif /* IO based transfer: calculate len */ if (fdctrl.fifo[5] == 00) { fdctrl.data_len = fdctrl.fifo[8]; } else { fdctrl.data_len = 128 << fdctrl.fifo[5]; } fdctrl.data_len *= cur_drv->last_sect; if (fdctrl.fifo[0] & 0x80) fdctrl.data_len *= 2; return; } static void fdctrl_start_transfer_del (int direction) { /* We don't handle deleted data, * so we don't return *ANYTHING* */ fdctrl_stop_transfer(0x60, 0x00); } int fdctrl_disk_change (int idx, const unsigned char *filename, int ro) { fdrive_t *drv; if (idx < 0 || idx > 1) return -1; drv = &fdctrl.drives[idx]; if (fd_table[idx] != NULL) { bdrv_close(fd_table[idx]); fd_table[idx] = NULL; } fd_table[idx] = bdrv_open(filename, ro); drv->bs = fd_table[idx]; if (fd_table[idx] == NULL) return -1; fd_revalidate(drv, ro); fd_recalibrate(drv); return 0; } void fdctrl_write_register (uint32_t reg, uint32_t value) { fdrive_t *cur_drv, *drv0, *drv1; drv0 = &fdctrl.drives[0]; drv1 = &fdctrl.drives[1]; cur_drv = fdctrl.cur_drv == 0 ? drv0 : drv1; #ifdef DEBUG_FLOPPY // printf("%s\n", __func__); #endif /* Reset mode */ if (fdctrl.state & FD_CTRL_RESET) { if (reg != 0x2 || !(value & 0x04)) return; #ifdef DEBUG_FLOPPY printf("Floppy controler out of RESET state\n"); #endif fdctrl.state &= ~FD_CTRL_RESET; } switch (reg) { case 0x2: /* Digital output register */ #ifdef DEBUG_FLOPPY printf("Floppy digital output register set to 0x%02x\n", value); #endif /* Drive motors state indicators */ if (value & 0x20) fd_start(drv1); else fd_stop(drv1); if (value & 0x10) fd_start(drv0); else fd_stop(drv0); /* DMA enable */ if (fdctrl.dma_chann != -1) fdctrl.dma_en = (value >> 3) & 1; /* Reset */ if (value & 0x04) { fdctrl.state |= FD_CTRL_RESET; fdctrl_reset(); fdctrl.state &= ~FD_CTRL_RESET; } /* Selected drive */ fdctrl.cur_drv = value & 1; break; case 0x3: /* Tape drive register */ #ifdef DEBUG_FLOPPY printf("Floppy tape drive register set to 0x%02x\n", value); #endif /* Disk boot selection indicator */ fdctrl.bootsel = (value >> 2) & 1; /* Tape indicators: never allow */ break; case 0x4: /* Data select rate register */ #ifdef DEBUG_FLOPPY printf("Floppy select rate register set to 0x%02x\n", value); #endif /* Reset */ if (value & 0x80) { fdctrl.state |= FD_CTRL_RESET; fdctrl_reset(); } if (value & 0x40) { fdctrl.state |= FD_CTRL_SLEEP; fdctrl_reset(); } // fdctrl.precomp = (value >> 2) & 0x07; break; case 0x5: /* Data register */ fdctrl.state &= ~FD_CTRL_SLEEP; /* Is it write command time ? */ if (fdctrl.data_state == FD_STATE_DATA) { /* FIFO data write */ fdctrl.fifo[fdctrl.data_pos++] = value; if (fdctrl.data_pos % FD_FIFO_LEN == (FD_FIFO_LEN - 1) || fdctrl.data_pos == fdctrl.data_len) { bdrv_write(cur_drv->bs, fd_sector(cur_drv), fdctrl.fifo, FD_FIFO_LEN); } /* Switch from transfert mode to status mode * then from status mode to command mode */ if (--fdctrl.data_state == 0x3) { fdctrl_stop_transfer(0x20, 0x00); fdctrl.data_dir = FD_DIR_READ; } return; } if (fdctrl.data_state != FD_STATE_CMD) break; if (fdctrl.data_pos == 0) { /* Command */ switch (value & 0x1F) { case 0x06: /* READ variants */ #ifdef DEBUG_FLOPPY printf("Floppy: READ command\n"); #endif /* 8 parameters cmd */ fdctrl.data_len = 9; goto enqueue; case 0x0C: /* READ_DELETED variants */ #ifdef DEBUG_FLOPPY printf("Floppy: READ_DELETED command\n"); #endif /* 8 parameters cmd */ fdctrl.data_len = 9; goto enqueue; case 0x10: /* SCAN_EQUAL variants */ #ifdef DEBUG_FLOPPY printf("Floppy: SCAN_EQUAL command\n"); #endif /* 8 parameters cmd */ fdctrl.data_len = 9; goto enqueue; case 0x16: /* VERIFY variants */ #ifdef DEBUG_FLOPPY printf("Floppy: VERIFY command\n"); #endif /* 8 parameters cmd */ fdctrl.data_len = 9; goto enqueue; case 0x19: /* SCAN_LOW_OR_EQUAL variants */ #ifdef DEBUG_FLOPPY printf("Floppy: SCAN_LOW_OR_EQUAL command\n"); #endif /* 8 parameters cmd */ fdctrl.data_len = 9; goto enqueue; case 0x1D: /* SCAN_HIGH_OR_EQUAL variants */ #ifdef DEBUG_FLOPPY printf("Floppy: SCAN_HIGH_OR_EQUAL command\n"); #endif /* 8 parameters cmd */ fdctrl.data_len = 9; goto enqueue; default: break; } switch (value & 0x3F) { case 0x05: /* WRITE variants */ #ifdef DEBUG_FLOPPY printf("Floppy: WRITE command\n"); #endif /* 8 parameters cmd */ fdctrl.data_len = 9; goto enqueue; case 0x09: /* WRITE_DELETED variants */ #ifdef DEBUG_FLOPPY printf("Floppy: WRITE_DELETED command\n"); #endif /* 8 parameters cmd */ fdctrl.data_len = 9; goto enqueue; default: break; } switch (value) { case 0x03: /* SPECIFY */ #ifdef DEBUG_FLOPPY printf("Floppy: SPECIFY command\n"); #endif /* 1 parameter cmd */ fdctrl.data_len = 2; goto enqueue; case 0x04: /* SENSE_DRIVE_STATUS */ #ifdef DEBUG_FLOPPY printf("Floppy: SENSE_DRIVE_STATUS command\n"); #endif /* 1 parameter cmd */ fdctrl.data_len = 2; goto enqueue; case 0x07: /* RECALIBRATE */ #ifdef DEBUG_FLOPPY printf("Floppy: RECALIBRATE command\n"); #endif /* 1 parameter cmd */ fdctrl.data_len = 2; goto enqueue; case 0x08: /* SENSE_INTERRUPT_STATUS */ #ifdef DEBUG_FLOPPY printf("Floppy: SENSE_INTERRUPT_STATUS command\n"); #endif /* No parameters cmd: returns status if no interrupt */ fdctrl.fifo[0] = (cur_drv->head << 2) | fdctrl.cur_drv; fdctrl.fifo[1] = cur_drv->track; fdctrl_set_state(2); return; case 0x0E: /* DATA_BUS */ #ifdef DEBUG_FLOPPY printf("Floppy: DATA_BUS command\n"); #endif /* Drives position */ fdctrl.fifo[0] = drv0->track; fdctrl.fifo[1] = drv1->track; fdctrl.fifo[2] = 0; fdctrl.fifo[3] = 0; /* timers */ fdctrl.fifo[4] = fdctrl.timer0; fdctrl.fifo[5] = (fdctrl.timer1 << 1) | fdctrl.dma_en; fdctrl.fifo[6] = cur_drv->last_sect; fdctrl.fifo[7] = (fdctrl.lock << 7) | (cur_drv->perpendicular << 2); fdctrl.fifo[8] = fdctrl.config; fdctrl.fifo[9] = fdctrl.precomp_trk; fdctrl_set_state(10); return; case 0x0F: /* SEEK */ #ifdef DEBUG_FLOPPY printf("Floppy: SEEK command\n"); #endif /* 2 parameters cmd */ fdctrl.data_len = 3; goto enqueue; case 0x10: /* VERSION */ #ifdef DEBUG_FLOPPY printf("Floppy: VERSION command\n"); #endif /* No parameters cmd */ /* Controler's version */ fdctrl.fifo[0] = fdctrl.version; fdctrl_set_state(1); return; case 12: /* PERPENDICULAR_MODE */ #ifdef DEBUG_FLOPPY printf("Floppy: PERPENDICULAR_MODE command\n"); #endif /* 1 parameter cmd */ fdctrl.data_len = 2; goto enqueue; case 0x13: /* CONFIGURE */ #ifdef DEBUG_FLOPPY printf("Floppy: CONFIGURE command\n"); #endif /* 3 parameters cmd */ fdctrl.data_len = 4; goto enqueue; case 0x14: /* UNLOCK */ #ifdef DEBUG_FLOPPY printf("Floppy: UNLOCK command\n"); #endif /* No parameters cmd */ fdctrl.lock = 0; fdctrl.fifo[0] = 0; fdctrl_set_state(1); return; case 0x17: /* POWERDOWN_MODE */ #ifdef DEBUG_FLOPPY printf("Floppy: POWERDOWN_MODE command\n"); #endif /* 2 parameters cmd */ fdctrl.data_len = 3; goto enqueue; case 0x18: /* PART_ID */ #ifdef DEBUG_FLOPPY printf("Floppy: PART_ID command\n"); #endif /* No parameters cmd */ fdctrl.fifo[0] = 0x43; /* Stepping 1 */ fdctrl_set_state(1); return; case 0x2C: /* SAVE */ #ifdef DEBUG_FLOPPY printf("Floppy: SAVE command\n"); #endif /* No parameters cmd */ fdctrl.fifo[0] = 0; fdctrl.fifo[1] = 0; /* Drives position */ fdctrl.fifo[2] = drv0->track; fdctrl.fifo[3] = drv1->track; fdctrl.fifo[4] = 0; fdctrl.fifo[5] = 0; /* timers */ fdctrl.fifo[6] = fdctrl.timer0; fdctrl.fifo[7] = fdctrl.timer1; fdctrl.fifo[8] = cur_drv->last_sect; fdctrl.fifo[9] = (fdctrl.lock << 7) | (cur_drv->perpendicular << 2); fdctrl.fifo[10] = fdctrl.config; fdctrl.fifo[11] = fdctrl.precomp_trk; fdctrl.fifo[12] = fdctrl.pwrd; fdctrl.fifo[13] = 0; fdctrl.fifo[14] = 0; fdctrl_set_state(15); return; case 0x33: /* OPTION */ #ifdef DEBUG_FLOPPY printf("Floppy: OPTION command\n"); #endif /* 1 parameter cmd */ fdctrl.data_len = 2; goto enqueue; case 0x42: /* READ_TRACK */ #ifdef DEBUG_FLOPPY printf("Floppy: READ_TRACK command\n"); #endif /* 8 parameters cmd */ fdctrl.data_len = 9; goto enqueue; case 0x4A: /* READ_ID */ #ifdef DEBUG_FLOPPY printf("Floppy: READ_ID command\n"); #endif /* 1 parameter cmd */ fdctrl.data_len = 2; goto enqueue; case 0x4C: /* RESTORE */ #ifdef DEBUG_FLOPPY printf("Floppy: RESTORE command\n"); #endif /* 17 parameters cmd */ fdctrl.data_len = 18; goto enqueue; case 0x4D: /* FORMAT_TRACK */ #ifdef DEBUG_FLOPPY printf("Floppy: FORMAT_TRACK command\n"); #endif /* 5 parameters cmd */ fdctrl.data_len = 6; goto enqueue; case 0x8E: /* DRIVE_SPECIFICATION_COMMAND */ #ifdef DEBUG_FLOPPY printf("Floppy: DRIVE_SPECIFICATION_COMMAND command\n"); #endif /* 5 parameters cmd */ fdctrl.data_len = 6; goto enqueue; case 0x8F: /* RELATIVE_SEEK_OUT */ #ifdef DEBUG_FLOPPY printf("Floppy: RELATIVE_SEEK_OUT command\n"); #endif /* 2 parameters cmd */ fdctrl.data_len = 3; goto enqueue; case 0x94: /* LOCK */ #ifdef DEBUG_FLOPPY printf("Floppy: LOCK command\n"); #endif /* No parameters cmd */ fdctrl.lock = 1; fdctrl.fifo[0] = 0x10; fdctrl_set_state(1); return; case 0xCD: /* FORMAT_AND_WRITE */ #ifdef DEBUG_FLOPPY printf("Floppy: FORMAT_AND_WRITE command\n"); #endif /* 10 parameters cmd */ fdctrl.data_len = 11; goto enqueue; case 0xCF: /* RELATIVE_SEEK_IN */ #ifdef DEBUG_FLOPPY printf("Floppy: RELATIVE_SEEK_IN command\n"); #endif /* 2 parameters cmd */ fdctrl.data_len = 3; goto enqueue; default: /* Unknown command */ #ifdef DEBUG_FLOPPY printf("Floppy: ERROR: unknown command: %d\n", value); #endif fdctrl_unimplemented(); return; } } enqueue: fdctrl.fifo[fdctrl.data_pos] = value; if (++fdctrl.data_pos == fdctrl.data_len) { /* We now have all parameters * and will be able to treat the command */ switch (fdctrl.fifo[0] & 0x1F) { case 0x06: { /* READ variants */ #ifdef DEBUG_FLOPPY printf("Floppy: treat READ command\n"); #endif fdctrl_start_transfer(1); return; } case 0x0C: /* READ_DELETED variants */ #ifdef DEBUG_FLOPPY printf("Floppy: treat READ_DELETED command\n"); #endif fdctrl_start_transfer_del(1); return; case 0x16: /* VERIFY variants */ #ifdef DEBUG_FLOPPY printf("Floppy: treat VERIFY command\n"); #endif fdctrl_start_transfer(2); return; case 0x10: /* SCAN_EQUAL variants */ #ifdef DEBUG_FLOPPY printf("Floppy: treat SCAN_EQUAL command\n"); #endif fdctrl_unimplemented(); return; case 0x19: /* SCAN_LOW_OR_EQUAL variants */ #ifdef DEBUG_FLOPPY printf("Floppy: treat SCAN_LOW_OR_EQUAL command\n"); #endif fdctrl_unimplemented(); return; case 0x1D: /* SCAN_HIGH_OR_EQUAL variants */ #ifdef DEBUG_FLOPPY printf("Floppy: treat SCAN_HIGH_OR_EQUAL command\n"); #endif fdctrl_unimplemented(); return; default: break; } switch (fdctrl.fifo[0] & 0x3F) { case 0x05: /* WRITE variants */ #ifdef DEBUG_FLOPPY printf("Floppy: treat WRITE command\n"); #endif fdctrl_start_transfer(0); return; case 0x09: /* WRITE_DELETED variants */ #ifdef DEBUG_FLOPPY printf("Floppy: treat WRITE_DELETED command\n"); #endif fdctrl_start_transfer_del(0); return; default: break; } switch (fdctrl.fifo[0]) { case 0x03: /* SPECIFY */ #ifdef DEBUG_FLOPPY printf("Floppy: treat SPECIFY command\n"); #endif fdctrl.timer0 = (fdctrl.fifo[1] >> 4) & 0xF; fdctrl.timer1 = fdctrl.fifo[1] >> 1; /* No result back */ fdctrl_reset_state(); return; case 0x04: /* SENSE_DRIVE_STATUS */ #ifdef DEBUG_FLOPPY printf("Floppy: treat SENSE_DRIVE_STATUS command\n"); #endif fdctrl.cur_drv = fdctrl.fifo[1] & 1; cur_drv = fdctrl.cur_drv == 0 ? drv0 : drv1; cur_drv->head = (fdctrl.fifo[1] >> 2) & 1; /* 1 Byte status back */ fdctrl.fifo[0] = 0x00; fdctrl_set_state(1); return; case 0x07: /* RECALIBRATE */ #ifdef DEBUG_FLOPPY printf("Floppy: treat RECALIBRATE command\n"); #endif fdctrl.cur_drv = fdctrl.fifo[1] & 1; cur_drv = fdctrl.cur_drv == 0 ? drv0 : drv1; cur_drv->track = 0; fdctrl_reset_state(); /* Raise Interrupt */ pic_set_irq(fdctrl.irq_lvl, 1); #if 1 __stb_cmmu(0x043E, __ldb_cmmu(0x043E, 0) | 0x80, 0); #endif return; case 0x0F: /* SEEK */ #ifdef DEBUG_FLOPPY printf("Floppy: treat SEEK command\n"); #endif fdctrl.cur_drv = fdctrl.fifo[1] & 1; cur_drv = fdctrl.cur_drv == 0 ? drv0 : drv1; if (fdctrl.fifo[2] <= cur_drv->track) cur_drv->dir = 1; else cur_drv->dir = 0; cur_drv->head = (fdctrl.fifo[1] >> 2) & 1; cur_drv->track = fdctrl.fifo[2]; fdctrl_reset_state(); /* Raise Interrupt */ pic_set_irq(fdctrl.irq_lvl, 1); #if 1 __stb_cmmu(0x043E, __ldb_cmmu(0x043E, 0) | 0x80, 0); #endif return; case 12: /* PERPENDICULAR_MODE */ #ifdef DEBUG_FLOPPY printf("Floppy: treat PERPENDICULAR_MODE command\n"); #endif if (fdctrl.fifo[1] & 0x80) cur_drv->perpendicular = fdctrl.fifo[1] & 0x7; /* No result back */ fdctrl_reset_state(); return; case 0x13: /* CONFIGURE */ #ifdef DEBUG_FLOPPY printf("Floppy: treat CONFIGURE command\n"); #endif fdctrl.config = fdctrl.fifo[2]; fdctrl.precomp_trk = fdctrl.fifo[3]; /* No result back */ fdctrl_reset_state(); return; case 0x17: /* POWERDOWN_MODE */ #ifdef DEBUG_FLOPPY printf("Floppy: treat POWERDOWN_MODE command\n"); #endif fdctrl.pwrd = fdctrl.fifo[1]; fdctrl.fifo[0] = fdctrl.fifo[1]; fdctrl_set_state(1); return; case 0x33: /* OPTION */ #ifdef DEBUG_FLOPPY printf("Floppy: treat OPTION command\n"); #endif /* No result back */ fdctrl_reset_state(); return; case 0x42: /* READ_TRACK */ #ifdef DEBUG_FLOPPY printf("Floppy: treat READ_TRACK command\n"); #endif fdctrl_unimplemented(); return; case 0x4A: /* READ_ID */ #ifdef DEBUG_FLOPPY printf("Floppy: treat READ_ID command\n"); #endif fdctrl_unimplemented(); return; case 0x4C: /* RESTORE */ #ifdef DEBUG_FLOPPY printf("Floppy: treat RESTORE command\n"); #endif /* Drives position */ drv0->track = fdctrl.fifo[3]; drv1->track = fdctrl.fifo[4]; /* timers */ fdctrl.timer0 = fdctrl.fifo[7]; fdctrl.timer1 = fdctrl.fifo[8]; cur_drv->last_sect = fdctrl.fifo[9]; fdctrl.lock = fdctrl.fifo[10] >> 7; cur_drv->perpendicular = (fdctrl.fifo[10] >> 2) & 0xF; fdctrl.config = fdctrl.fifo[11]; fdctrl.precomp_trk = fdctrl.fifo[12]; fdctrl.pwrd = fdctrl.fifo[13]; fdctrl_reset_state(); return; case 0x4D: /* FORMAT_TRACK */ #ifdef DEBUG_FLOPPY printf("Floppy: treat FORMAT_TRACK command\n"); #endif fdctrl_unimplemented(); return; case 0x8E: /* DRIVE_SPECIFICATION_COMMAND */ #ifdef DEBUG_FLOPPY printf("Floppy: treat DRIVE_SPECIFICATION_COMMAND command\n"); #endif if (fdctrl.fifo[fdctrl.data_pos - 1] & 0x80) { /* Command parameters done */ if (fdctrl.fifo[fdctrl.data_pos - 1] & 0x40) { fdctrl.fifo[0] = fdctrl.fifo[1]; fdctrl.fifo[2] = 0; fdctrl.fifo[3] = 0; fdctrl_set_state(4); } else { fdctrl_reset_state(); } } else if (fdctrl.data_len > 7) { /* ERROR */ fdctrl.fifo[0] = 0x80 | (cur_drv->head << 2) | fdctrl.cur_drv; fdctrl_set_state(1); } return; case 0x8F: /* RELATIVE_SEEK_OUT */ #ifdef DEBUG_FLOPPY printf("Floppy: treat RELATIVE_SEEK_OUT command\n"); #endif fdctrl.cur_drv = fdctrl.fifo[1] & 1; cur_drv = fdctrl.cur_drv == 0 ? drv0 : drv1; cur_drv->head = (fdctrl.fifo[1] >> 2) & 1; if (fdctrl.fifo[2] + cur_drv->track > cur_drv->max_track) { /* ERROR */ fdctrl.fifo[0] = 0x50 | (cur_drv->head << 2) | fdctrl.cur_drv; fdctrl_set_state(1); } else { cur_drv->track += fdctrl.fifo[2]; cur_drv->dir = 0; fdctrl_reset_state(); pic_set_irq(fdctrl.irq_lvl, 1); #if 1 __stb_cmmu(0x043E, __ldb_cmmu(0x043E, 0) | 0x80, 0); #endif } return; case 0xCD: /* FORMAT_AND_WRITE */ #ifdef DEBUG_FLOPPY printf("Floppy: treat FORMAT_AND_WRITE command\n"); #endif fdctrl_unimplemented(); return; case 0xCF: /* RELATIVE_SEEK_IN */ #ifdef DEBUG_FLOPPY printf("Floppy: treat RELATIVE_SEEK_IN command\n"); #endif fdctrl.cur_drv = fdctrl.fifo[1] & 1; cur_drv = fdctrl.cur_drv == 0 ? drv0 : drv1; cur_drv->head = (fdctrl.fifo[1] >> 2) & 1; if (fdctrl.fifo[2] > cur_drv->track) { /* ERROR */ fdctrl.fifo[0] = 0x50 | (cur_drv->head << 2) | fdctrl.cur_drv; fdctrl_set_state(1); } else { fdctrl_reset_state(); cur_drv->track -= fdctrl.fifo[2]; cur_drv->dir = 1; /* Raise Interrupt */ pic_set_irq(fdctrl.irq_lvl, 1); #if 1 __stb_cmmu(0x043E, __ldb_cmmu(0x043E, 0) | 0x80, 0); #endif } return; } } break; case 0x7: #ifdef DEBUG_FLOPPY printf("Floppy digital input register id read-only\n"); #endif break; case 0x1: /* Status register B */ #ifdef DEBUG_FLOPPY printf("Floppy state B register is read-only\n"); #endif break; default: printf("ERROR: attempt to write to an unknown floppy port: %x\n", reg); break; } } uint32_t fdctrl_read_register (uint32_t reg) { fdrive_t *cur_drv, *drv0, *drv1; uint32_t retval = 0; int pos, len; drv0 = &fdctrl.drives[0]; drv1 = &fdctrl.drives[1]; cur_drv = fdctrl.cur_drv == 0 ? drv0 : drv1; switch (reg) { case 0x1: #ifdef DEBUG_FLOPPY printf("Floppy status register: 0x%02x\n", retval); #endif break; case 0x2: /* Digital output register */ /* Drive motors state indicators */ retval |= drv1->motor << 5; retval |= drv0->motor << 4; /* DMA enable */ retval |= fdctrl.dma_en << 3; /* Reset indicator */ retval |= (fdctrl.state & FD_CTRL_RESET) == 0 ? 0x04 : 0; /* Selected drive */ retval |= fdctrl.cur_drv; #ifdef DEBUG_FLOPPY printf("Floppy digital output register: 0x%02x\n", retval); #endif break; case 0x3: /* Tape drive register */ /* Disk boot selection indicator */ retval |= fdctrl.bootsel << 2; /* Tape indicators: never allowed */ #ifdef DEBUG_FLOPPY printf("Floppy tape drive register: 0x%02x\n", retval); #endif break; case 0x4: /* Main status register */ fdctrl.state &= ~FD_CTRL_SLEEP; if (!(fdctrl.state & FD_CTRL_BUSY)) { /* Data transfer allowed */ retval |= 0x80; /* Data transfer direction indicator */ if (fdctrl.data_dir == FD_DIR_READ) retval |= 0x40; } /* Should handle 0x20 for SPECIFY command */ /* Command busy indicator */ if (fdctrl.data_state == FD_STATE_DATA || fdctrl.data_state == FD_STATE_STATUS) retval |= 0x10; #ifdef DEBUG_FLOPPY printf("Floppy main status register: 0x%02x\n", retval); #endif break; case 0x5: /* Data register */ fdctrl.state &= ~FD_CTRL_SLEEP; if (fdctrl.data_state != FD_STATE_CMD) { pos = fdctrl.data_pos; if (fdctrl.data_state == FD_STATE_DATA) { pos %= FD_FIFO_LEN; if (pos == 0) { len = fdctrl.data_len - fdctrl.data_pos; if (len > FD_FIFO_LEN) len = FD_FIFO_LEN; bdrv_read(cur_drv->bs, fd_sector(cur_drv), fdctrl.fifo, len); } } retval = fdctrl.fifo[pos]; if (++fdctrl.data_pos == fdctrl.data_len) { fdctrl.data_pos = 0; /* Switch from transfert mode to status mode * then from status mode to command mode */ if (--fdctrl.data_state == FD_STATE_STATUS) { fdctrl_stop_transfer(0x20, 0x00); } else { fdctrl.data_dir = FD_DIR_WRITE; } } } #ifdef DEBUG_FLOPPY printf("Floppy data register: 0x%02x\n", retval); #endif break; case 0x7: /* Digital input register */ #ifdef DEBUG_FLOPPY printf("Floppy digital input register: 0x%02x\n", retval); #endif if (drv0->rv | drv1->rv) retval |= 0x80; drv0->rv = 0; drv1->rv = 0; break; default: printf("ERROR: attempt to read from an unknown floppy port: %x\n", reg); break; } return retval; } -- J. Mayer Never organized