/* seek to track */
static int seek(int track, int head)
{
//LOG("seek() called ...\n");
/*
if (track == 0) {
LOG("RECALIBRATE...\n");
recalibrate();
return TRUE;
}
*/
if (fdc_track == track)
return TRUE; /* already there*/
/* send actual command bytes */
send_byte(FD_SEEK);
send_byte(head << 2);
send_byte(track);
/* wait until seek finished */
if ( !wait_fdc(TRUE) )
;//return FALSE; /* time out */
//LOG("ST0: %x\t ST1: %x\n", sr0, fdc_track);
if ( ((sr0 & 0xF8) != 0x20) || (fdc_track != track)) {
LOG("Seek track#: %d failed\n", track);
return FALSE;
} else {
LOG("Seek track#: %d OK ...\n", track);
return TRUE;
}
}
/* recalibrate the drive */
static void recalibrate(void)
{
//LOG("recalibrate() called ...\n");
/*turn the motor on first */
motor_on();
/* send actual command bytes */
send_byte(FD_RECALIBRATE);
send_byte(0);
/* wait until seek finished */
wait_fdc(TRUE);
}
static void recalibrate(struct fdd *d)
{
int i;
for (i = 0; i < 13; i++) {
fdc_out(d->fdc->base_port, FDC_RECALIBRATE);
fdc_out(d->fdc->base_port, d->number);
wait_fdc(d);
/* Send a `sense interrupt status' command */
fdc_out(d->fdc->base_port, FDC_SENSE);
d->fdc->sr0 = fdc_in(d->fdc->base_port);
d->track = fdc_in(d->fdc->base_port);
if (!(d->fdc->sr0 & 0x10)) break; // Exit if unit check is not set
}
DEBUG(DL_DBG, ("drive(%d), sr0(0x%x), track(%d)\n",
d->number, d->fdc->sr0, d->track));
}
int flpy_seek(struct fdd *d, uint32_t track)
{
if (d->track == track) return 0;
fdc_out(d->fdc->base_port, FDC_SEEK);
fdc_out(d->fdc->base_port, d->number);
fdc_out(d->fdc->base_port, track);
if (wait_fdc(d)) return -1; // Timeout
/* Send a `sense interrupt status' command */
fdc_out(d->fdc->base_port, FDC_SENSE);
d->fdc->sr0 = fdc_in(d->fdc->base_port);
d->track = fdc_in(d->fdc->base_port);
/* Check that seek worked */
if ((d->fdc->sr0 != 0x20 + d->number) || (d->track != track)) {
DEBUG(DL_ERR, ("error on drive(%d)\n"
"* sr0(0x%x), track(%d), expected(%d)\n",
d->number, d->fdc->sr0, d->track, track));
return -1; // TODO: return an error code
}
return 0;
}
/*
* reset the floppy.
*
* The first thing that the driver needs to do is reset the controller.This
* will put it in a known state. To reset the primary floppy controller,(in C)
*
* 1.write 0x00 to the DIGITAL_OUTPUT_REG of the desired controller
* 2.write 0x0C to the DIGITAL_OUTPUT_REG of the desired controller
* 3.wait for an interrupt from the controller
* 4.check interrupt status (this is function 0x08 of controllers)
* 5.write 0x00 to the CONFIG_CONTROL_REG
* 6.configure the drive desired on the controller (function 0x03 of controller)
* 7.calibrate the drive (function 0x07 of controller)
*
*/
static void reset( )
{
//LOG("reset() called ...\n");
/* stop the motor and disable IRQ/DMA */
outb_p(0x0c,FD_DOR);
/* program data rate (500K/s) */
outb_p(0,FD_DCR);
/* re-enable interrupts */
outb_p(0x1c,FD_DOR);
/* resetting triggered an interrupt - handle it */
done = TRUE;
wait_fdc(TRUE);
/* specify drive timings (got these off the BIOS) */
send_byte(FD_SPECIFY);
send_byte(0xdf); /* SRT = 3ms, HUT = 240ms */
send_byte(0x06); /* HLT = 16ms, ND = 0 */
recalibrate();
}
/*
* And now, it's time to implenent the read or write function, that's
* all the floppy driver mean!
*
* Read/Write one sector once.
*/
static int floppy_rw(int sector, char *buf, int command)
{
int head;
char *dma_buffer = buf;
static char tmp_dma_buffer[512];
//LOG("TMP dma buffer: %p\n", tmp_dma_buffer);
lba_to_chs(sector, &head, &track, §or);
LOG("head: %d \ttrack: %d \tsector: %d\n", head, track, sector);
/* turn it on if not */
motor_on();
if (inb_p(FD_DIR) & 0x80) {
changed = TRUE;
seek(1, head); /* clear "disk change" status */
recalibrate();
motor_off();
printk("floppy_rw: Disk change detected. You are going to DIE:)\n");
pause(); /* just put it in DIE */
}
/* move head to the right track */
if (!seek(track, head)) {
motor_off();
printk("floppy_rw: Error seeking to track#%d\n", track);
return FALSE;
}
if ((unsigned long)buf >= 0xff000) {
dma_buffer = tmp_dma_buffer;
if (command == FD_WRITE)
memcpy(dma_buffer, buf, 512);
}
setup_DMA((unsigned long)dma_buffer, command);
send_byte(command);
send_byte(head<<2 | 0);
send_byte(track);
send_byte(head);
send_byte(sector);
send_byte(2); /* sector size = 125 * 2^(2) */
send_byte(floppy.sector);
send_byte(0);
send_byte(0xFF); /* sector size(only two valid vaules, 0xff when n!=0*/
if (!wait_fdc(FALSE)) {
//LOG("wait fdc failed!\n");
//return 0;
/*
printk("Time out, trying operation again after reset() \n");
reset();
return floppy_rw(sector, buf, command);
*/
}
motor_off();
if (/*res != 7 || */(ST0 & 0xf8) || (ST1 & 0xbf) || (ST2 & 0x73) ) {
if (ST1 & 0x02)
LOG("Drive is write protected!\n");
else
LOG("floppy_rw: bad interrupt!\n");
return -EIO;
} else {
LOG("floppy_rw: OK\n");
if ((unsigned long)buf >= 0xff000 && command == FD_READ)
memcpy(buf, dma_buffer, 512);
return 0;
}
}
