X-Git-Url: https://git.ucc.asn.au/?a=blobdiff_plain;f=Modules%2FStorage%2FFDD%2Ffdd.c;h=9cf808d26e8768548f4bc52e9931101537e96101;hb=a2495c6ea4f4cab16b5d339ae511428e92e89e73;hp=149465e3dbdabd36236c45b60c429da112a0929f;hpb=6a945643557084578509e149c84cf5dde3c59c3c;p=tpg%2Facess2.git

diff --git a/Modules/Storage/FDD/fdd.c b/Modules/Storage/FDD/fdd.c
index 149465e3..9cf808d2 100644
--- a/Modules/Storage/FDD/fdd.c
+++ b/Modules/Storage/FDD/fdd.c
@@ -6,7 +6,7 @@
 #include <acess.h>
 #include <modules.h>
 #include <fs_devfs.h>
-#include <tpl_drv_disk.h>
+#include <api_drv_disk.h>
 #include <dma.h>
 #include <iocache.h>
 
@@ -62,24 +62,34 @@ enum FloppyPorts {
 	PORT_DIGOUTPUT	= 0x2,
 	PORT_MAINSTATUS	= 0x4,
 	PORT_DATARATE	= 0x4,
-	PORT_DATA		= 0x5,
+	PORT_DATA   	= 0x5,
 	PORT_DIGINPUT	= 0x7,
 	PORT_CONFIGCTRL	= 0x7
 };
 
+#define CMD_FLAG_MULTI_TRACK	0x80	//!< Multitrack
+#define CMD_FLAG_MFM_ENCODING	0x40	//!< MFM Encoding Mode (Always set for read/write/format/verify)
+#define CMD_FLAG_SKIP_MODE  	0x20	//!< Skip Mode (don't use)
 enum FloppyCommands {
-	FIX_DRIVE_DATA	= 0x03,
-	HECK_DRIVE_STATUS	= 0x04,
-	CALIBRATE_DRIVE	= 0x07,
-	CHECK_INTERRUPT_STATUS = 0x08,
-	SEEK_TRACK		= 0x0F,
-	READ_SECTOR_ID	= 0x4A,
-	FORMAT_TRACK	= 0x4D,
-	READ_TRACK		= 0x42,
-	READ_SECTOR		= 0x66,
-	WRITE_SECTOR	= 0xC5,
-	WRITE_DELETE_SECTOR	= 0xC9,
-	READ_DELETE_SECTOR	= 0xCC,
+	CMD_READ_TRACK      = 0x02,
+	CMD_SPECIFY         = 0x03,
+	CMD_SENSE_STATUS    = 0x04,
+	CMD_WRITE_DATA      = 0x05,
+	CMD_READ_DATA       = 0x06,
+	CMD_RECALIBRATE     = 0x07,
+	CMD_SENSE_INTERRUPT = 0x08,
+	CMD_WRITE_DEL_DATA  = 0x09,
+	CMD_READ_SECTOR_ID  = 0x0A,
+	// 0x0B - ?
+	CMD_READ_DEL_DATA	= 0x0C,
+	CMD_FORMAT_TRACK    = 0x0D,
+	// 0x0E - ?
+	CMD_SEEK_TRACK      = 0x0F,
+	CMD_VERSION         = 0x10,
+	
+	CMD_LOCK            = 0x14,	//!< Save controller parameters
+	
+	CMD_CONFIGURE       = 0x13
 };
 
 // === PROTOTYPES ===
@@ -97,23 +107,28 @@ Uint	FDD_ReadSectors(Uint64 SectorAddr, Uint Count, void *Buffer, Uint Disk);
  int	FDD_ReadSector(Uint32 disk, Uint64 lba, void *Buffer);
  int	FDD_WriteSector(Uint32 Disk, Uint64 LBA, void *Buffer);
 // --- Helpers
-void	FDD_IRQHandler(int Num);
-inline void	FDD_WaitIRQ();
-void	FDD_SensInt(int base, Uint8 *sr0, Uint8 *cyl);
-void	FDD_int_SendByte(int base, char byte);
- int	FDD_int_GetByte(int base);
-void	FDD_Reset(int id);
+ int	FDD_WaitIRQ();
+void	FDD_IRQHandler(int Num, void *Ptr);
+void	FDD_SenseInt(int base, Uint8 *sr0, Uint8 *cyl);
+
+ int	FDD_Reset(int id);
 void	FDD_Recalibrate(int disk);
+ int	FDD_Reconfigure(int ID);
+
  int	FDD_int_SeekTrack(int disk, int head, int track);
 void	FDD_int_TimerCallback(void *Arg);
-void	FDD_int_StopMotor(void *Arg);
+void	FDD_int_StopMotor(int Disk);
+void	FDD_int_StopMotorCallback(void *Arg);
 void	FDD_int_StartMotor(int Disk);
  int	FDD_int_GetDims(int type, int lba, int *c, int *h, int *s, int *spt);
 
+ int	FDD_int_SendByte(int base, Uint8 Byte);
+ int	FDD_int_GetByte(int base, Uint8 *Byte);
+
 // === GLOBALS ===
-MODULE_DEFINE(0, FDD_VERSION, FDD, FDD_Install, NULL, "ISADMA", NULL);
+MODULE_DEFINE(0, FDD_VERSION, FDD, FDD_Install, NULL, "x86_ISADMA", NULL);
 t_floppyDevice	gFDD_Devices[2];
-tSpinlock	glFDD;
+tMutex	glFDD;
 volatile int	gbFDD_IrqFired = 0;
 tDevFS_Driver	gFDD_DriverInfo = {
 	NULL, "fdd",
@@ -146,6 +161,13 @@ int FDD_Install(char **Arguments)
 	gFDD_Devices[0].track[0] = -1;
 	gFDD_Devices[1].track[1] = -1;
 	
+	Log_Log("FDD", "Detected Disk 0: %s and Disk 1: %s", cFDD_TYPES[data>>4], cFDD_TYPES[data&0xF]);
+	
+	if( data == 0 ) {
+		return MODULE_ERR_NOTNEEDED;
+	}
+	
+	// Handle arguments
 	if(args) {
 		for(;*args;args++)
 		{
@@ -154,18 +176,45 @@ int FDD_Install(char **Arguments)
 		}
 	}
 	
-	Log_Log("FDD", "Detected Disk 0: %s and Disk 1: %s", cFDD_TYPES[data>>4], cFDD_TYPES[data&0xF]);
-	
-	if( data == 0 ) {
-		return MODULE_ERR_NOTNEEDED;
-	}
-	
-	// Clear FDD IRQ Flag
-	FDD_SensInt(0x3F0, NULL, NULL);
 	// Install IRQ6 Handler
-	IRQ_AddHandler(6, FDD_IRQHandler);
+	IRQ_AddHandler(6, FDD_IRQHandler, NULL);
+
+	// Ensure the FDD version is 0x90
+	{
+		Uint8	tmp = 0;
+		FDD_int_SendByte(cPORTBASE[0], CMD_VERSION);
+		FDD_int_GetByte(cPORTBASE[0], &tmp);
+		if( tmp != 0x90 ) {
+			Log_Error("FDD", "Version(0x%2x) != 0x90", tmp);
+			return MODULE_ERR_NOTNEEDED;
+		}
+	}
+
+	// Configure
+	FDD_Reconfigure(0);
+
 	// Reset Primary FDD Controller
-	FDD_Reset(0);
+	if( FDD_Reset(0) != 0 ) {
+		return MODULE_ERR_MISC;
+	}
+
+	#if 0
+	{
+		 int	retries;
+		// Recalibrate disks
+		LOG("Recalibrate disks (16x seek)");
+		retries = 16;
+		while(FDD_int_SeekTrack(0, 0, 1) == 0 && retries --)
+			Threads_Yield();	// set track
+		if(retries < 0)	LEAVE_RET('i', -1);
+	
+		retries = 16;
+		while(FDD_int_SeekTrack(0, 1, 1) == 0 && retries --)
+			Threads_Yield();	// set track
+		if(retries < 0)	LEAVE_RET('i', -1);
+	}
+	#endif
+	
 	
 	// Initialise Root Node
 	gFDD_DriverInfo.RootNode.CTime = gFDD_DriverInfo.RootNode.MTime
@@ -213,13 +262,13 @@ int FDD_Install(char **Arguments)
 void FDD_UnloadModule()
 {
 	 int	i;
-	//DevFS_DelDevice( &gFDD_DriverInfo );
-	LOCK(&glFDD);
+	DevFS_DelDevice( &gFDD_DriverInfo );
+	Mutex_Acquire(&glFDD);
 	for(i=0;i<4;i++) {
 		Time_RemoveTimer(gFDD_Devices[i].timer);
-		FDD_int_StopMotor((void *)(Uint)i);
+		FDD_int_StopMotor(i);
 	}
-	RELEASE(&glFDD);
+	Mutex_Release(&glFDD);
 	//IRQ_Clear(6);
 }
 
@@ -355,7 +404,7 @@ int FDD_int_ReadWriteSector(Uint32 Disk, Uint64 SectorAddr, int Write, void *Buf
 	 int	spt, base;
 	 int	i;
 	 int	lba = SectorAddr;
-	Uint8	st0, st1, st2, rcy, rhe, rse, bps;	//	Status Values
+	Uint8	st0=0, st1=0, st2=0, bps=0;	// Status Values
 	
 	ENTER("iDisk XSectorAddr pBuffer", Disk, SectorAddr, Buffer);
 	
@@ -369,67 +418,57 @@ int FDD_int_ReadWriteSector(Uint32 Disk, Uint64 SectorAddr, int Write, void *Buf
 		return -1;
 	}
 	LOG("Cyl=%i, Head=%i, Sector=%i", cyl, head, sec);
-	
-	LOCK(&glFDD);	// Lock to stop the motor stopping on us
+
+	// Start the motor	
+	Mutex_Acquire(&glFDD);	// Lock to stop the motor stopping on us
 	Time_RemoveTimer(gFDD_Devices[Disk].timer);	// Remove Old Timer
 	// Start motor if needed
 	if(gFDD_Devices[Disk].motorState != 2)	FDD_int_StartMotor(Disk);
-	RELEASE(&glFDD);
-	
-	LOG("Wait for the motor to spin up");
+	Mutex_Release(&glFDD);
 	
 	// Wait for spinup
+	LOG("Wait for the motor to spin up");
 	while(gFDD_Devices[Disk].motorState == 1)	Threads_Yield();
 	
 	LOG("Acquire Spinlock");
-	LOCK(&glFDD);
+	Mutex_Acquire(&glFDD);
 	
-	// Seek to track
-	outb(base + CALIBRATE_DRIVE, 0);
-	i = 0;
-	while(FDD_int_SeekTrack(Disk, head, (Uint8)cyl) == 0 && i++ < FDD_SEEK_TIMEOUT )
-		Threads_Yield();
-	if( i > FDD_SEEK_TIMEOUT ) {
-		RELEASE(&glFDD);
-		LEAVE('i', 0);
-		return 0;
-	}
-	//FDD_SensInt(base, NULL, NULL);	// Wait for IRQ
-		
 	// Read Data from DMA
 	LOG("Setting DMA for read");
-	DMA_SetChannel(2, 512, !Write);	// Read 512 Bytes from channel 2
+	DMA_SetChannel(2, 512, !Write);	// Read/Write 512 Bytes from channel 2
 	
 	LOG("Sending command");
 	
-	//Threads_Wait(100);	// Wait for Head to settle
-	Time_Delay(100);
-	
+	#define SENDB(__data)	if(FDD_int_SendByte(base, __data)) { FDD_Reset(Disk >> 1); continue; }
+
 	for( i = 0; i < FDD_MAX_READWRITE_ATTEMPTS; i ++ )
 	{
-		if( Write )
-			FDD_int_SendByte(base, READ_SECTOR);	// Was 0xE6
-		else
-			FDD_int_SendByte(base, READ_SECTOR);	// Was 0xE6
-		FDD_int_SendByte(base, (head << 2) | (Disk&1));
-		FDD_int_SendByte(base, (Uint8)cyl);
-		FDD_int_SendByte(base, (Uint8)head);
-		FDD_int_SendByte(base, (Uint8)sec);
-		FDD_int_SendByte(base, 0x02);	// Bytes Per Sector (Real BPS=128*2^{val})
-		FDD_int_SendByte(base, spt);	// SPT
-		FDD_int_SendByte(base, 0x1B);	// Gap Length (27 is default)
-		FDD_int_SendByte(base, 0xFF);	// Data Length
+		FDD_int_SeekTrack(Disk, head, cyl);
+		if( Write ) {
+			SENDB(CMD_WRITE_DATA|CMD_FLAG_MFM_ENCODING);
+		}
+		else {
+			SENDB(CMD_READ_DATA|CMD_FLAG_MFM_ENCODING);
+		}
+		SENDB( (head << 2) | (Disk&1) );
+		SENDB(cyl & 0xFF);
+		SENDB(head & 0xFF);
+		SENDB(sec & 0xFF);
+		SENDB(0x02);	// Bytes Per Sector (Real BPS=128*2^{val})
+		SENDB(spt);	// SPT
+		SENDB(0x1B);	// Gap Length (27 is default)
+		SENDB(0xFF);	// Data Length
 		
 		// Wait for IRQ
 		if( Write ) {
 			LOG("Writing Data");
 			DMA_WriteData(2, 512, Buffer);
 			LOG("Waiting for Data to be written");
-			FDD_WaitIRQ();
+			if( FDD_WaitIRQ() ) { FDD_Reset(Disk>>1); continue; }
 		}
 		else {
 			LOG("Waiting for data to be read");
-			FDD_WaitIRQ();
+			if( FDD_WaitIRQ() ) { FDD_Reset(Disk>>1); continue; }
 			LOG("Reading Data");
 			DMA_ReadData(2, 512, Buffer);
 		}
@@ -437,26 +476,26 @@ int FDD_int_ReadWriteSector(Uint32 Disk, Uint64 SectorAddr, int Write, void *Buf
 		// Clear Input Buffer
 		LOG("Clearing Input Buffer");
 		// Status Values
-		st0 = FDD_int_GetByte(base);
-		st1 = FDD_int_GetByte(base);
-		st2 = FDD_int_GetByte(base);
+		FDD_int_GetByte(base, &st0);
+		FDD_int_GetByte(base, &st1);
+		FDD_int_GetByte(base, &st2);
 		
-		// Cylinder, Head and Sector (mutilated in some way
-		rcy = FDD_int_GetByte(base);
-		rhe = FDD_int_GetByte(base);
-		rse = FDD_int_GetByte(base);
+		// Cylinder, Head and Sector (mutilated in some way)
+		FDD_int_GetByte(base, NULL);	// Cylinder
+		FDD_int_GetByte(base, NULL);	// Head
+		FDD_int_GetByte(base, NULL);	// Sector
 		// Should be the BPS set above (0x02)
-		bps = FDD_int_GetByte(base);
+		FDD_int_GetByte(base, &bps);
 		
 		// Check Status
 		// - Error Code
 		if(st0 & 0xC0) {
 			LOG("Error (st0 & 0xC0) \"%s\"", cFDD_STATUSES[st0 >> 6]);
 			continue;
-	    }
-	    // - Status Flags
-	    if(st0 & 0x08) {	LOG("Drive not ready");	continue; 	}
-	    if(st1 & 0x80) {	LOG("End of Cylinder");	continue;	}
+		}
+		// - Status Flags
+		if(st0 & 0x08) {	LOG("Drive not ready");	continue; 	}
+		if(st1 & 0x80) {	LOG("End of Cylinder");	continue;	}
 		if(st1 & 0x20) {	LOG("CRC Error");	continue;	}
 		if(st1 & 0x10) {	LOG("Controller Timeout");	continue;	}
 		if(st1 & 0x04) {	LOG("No Data Found");	continue;	}
@@ -464,7 +503,7 @@ int FDD_int_ReadWriteSector(Uint32 Disk, Uint64 SectorAddr, int Write, void *Buf
 			LOG("No Address mark found");
 			continue;
 		}
-	    if(st2 & 0x40) {	LOG("Deleted address mark");	continue;	}
+		if(st2 & 0x40) {	LOG("Deleted address mark");	continue;	}
 		if(st2 & 0x20) {	LOG("CRC error in data");	continue;	}
 		if(st2 & 0x10) {	LOG("Wrong Cylinder");	continue;	}
 		if(st2 & 0x04) {	LOG("uPD765 sector not found");	continue;	}
@@ -477,6 +516,7 @@ int FDD_int_ReadWriteSector(Uint32 Disk, Uint64 SectorAddr, int Write, void *Buf
 		
 		if(st1 & 0x02) {
 			LOG("Floppy not writable");
+			// Return error without triggering the attempt count check
 			i = FDD_MAX_READWRITE_ATTEMPTS+1;
 			break;
 		}
@@ -484,10 +524,11 @@ int FDD_int_ReadWriteSector(Uint32 Disk, Uint64 SectorAddr, int Write, void *Buf
 		// Success!
 		break;
 	}
+	#undef SENDB
 	
 	// Release Spinlock
 	LOG("Realeasing Spinlock and setting motor to stop");
-	RELEASE(&glFDD);
+	Mutex_Release(&glFDD);
 	
 	if(i == FDD_MAX_READWRITE_ATTEMPTS) {
 		Log_Warning("FDD", "Exceeded %i attempts in %s the disk",
@@ -497,8 +538,9 @@ int FDD_int_ReadWriteSector(Uint32 Disk, Uint64 SectorAddr, int Write, void *Buf
 	}
 	
 	// Don't turn the motor off now, wait for a while
-	gFDD_Devices[Disk].timer = Time_CreateTimer(MOTOR_OFF_DELAY, FDD_int_StopMotor, (void*)(tVAddr)Disk);
+	FDD_int_StopMotor(Disk);
 
+	// Error check
 	if( i < FDD_MAX_READWRITE_ATTEMPTS ) {
 		LEAVE('i', 0);
 		return 0;
@@ -513,7 +555,7 @@ int FDD_int_ReadWriteSector(Uint32 Disk, Uint64 SectorAddr, int Write, void *Buf
  * \fn int FDD_ReadSector(Uint32 Disk, Uint64 SectorAddr, void *Buffer)
  * \brief Read a sector from disk
  * \todo Make real-hardware safe (account for read errors)
-*/
+ */
 int FDD_ReadSector(Uint32 Disk, Uint64 SectorAddr, void *Buffer)
 {
 	 int	ret;
@@ -547,18 +589,17 @@ int FDD_ReadSector(Uint32 Disk, Uint64 SectorAddr, void *Buffer)
  */
 int FDD_WriteSector(Uint32 Disk, Uint64 LBA, void *Buffer)
 {
-	Warning("[FDD  ] Read Only at the moment");
+	Log_Warning("FDD", "Read Only at the moment");
 	return -1;
 }
 
 /**
- * \fn int FDD_int_SeekTrack(int disk, int track)
  * \brief Seek disk to selected track
  */
 int FDD_int_SeekTrack(int disk, int head, int track)
 {
-	Uint8	sr0, cyl;
-	 int	base;
+	Uint8	sr0=0, cyl=0;
+	 int	base, i, bUnclean;
 	
 	base = cPORTBASE[disk>>1];
 	
@@ -567,19 +608,40 @@ int FDD_int_SeekTrack(int disk, int head, int track)
 		return 1;
 	
 	// - Seek Head 0
-	FDD_int_SendByte(base, SEEK_TRACK);
-	FDD_int_SendByte(base, (head<<2)|(disk&1));
-	FDD_int_SendByte(base, track);	// Send Seek command
-	FDD_WaitIRQ();
-	FDD_SensInt(base, &sr0, &cyl);	// Wait for IRQ
-	if((sr0 & 0xF0) != 0x20) {
-		LOG("sr0 = 0x%x", sr0);
-		return 0;	//Check Status
-	}
-	if(cyl != track)	return 0;
+	for( i = 0; i < FDD_MAX_READWRITE_ATTEMPTS; i ++ )
+	{
+		if(i && bUnclean)	FDD_Reset(disk >> 1);
+		bUnclean = 1;
+		if(FDD_int_SendByte(base, CMD_SEEK_TRACK))	continue;
+		if(FDD_int_SendByte(base, (head<<2)|(disk&1)))	continue;
+		if(FDD_int_SendByte(base, track))	continue;
+		FDD_WaitIRQ();
+		FDD_SenseInt(base, &sr0, &cyl);	// Wait for IRQ
+
+		bUnclean = 0;
+		if( cyl != track )
+			continue;	// Try again
+		if( (sr0 & 0xF0) != 0x20 ) {
+			LOG("sr0 = 0x%x", sr0);
+			continue ;
+		}
 	
+		break;
+	}
+
+	if( i == FDD_MAX_READWRITE_ATTEMPTS ) {
+		Log_Warning("FDD", "Unable to seek to track %i on disk %i",
+			track, disk);
+		return 0;
+	}	
+
 	// Set Track in structure
 	gFDD_Devices[disk].track[head] = track;
+
+	LOG("Time_Delay(100)");	
+	// Wait for Head to settle
+	Time_Delay(100);
+	
 	return 1;
 }
 
@@ -644,71 +706,93 @@ int FDD_int_GetDims(int type, int lba, int *c, int *h, int *s, int *spt)
  * \fn void FDD_IRQHandler(int Num)
  * \brief Handles IRQ6
  */
-void FDD_IRQHandler(int Num)
+void FDD_IRQHandler(int Num, void *Ptr)
 {
 	gbFDD_IrqFired = 1;
 }
 
 /**
- * \fn FDD_WaitIRQ()
- * \brief Wait for an IRQ6
+ * \brief Wait for the FDD IRQ to fire
+ * \return Boolean failure (1 for timeout)
  */
-inline void FDD_WaitIRQ()
+inline int FDD_WaitIRQ()
 {
+	tTime	end = now() + 2000;
+	
 	// Wait for IRQ
-	while(!gbFDD_IrqFired)	Threads_Yield();
+	while(!gbFDD_IrqFired && now() < end)
+		Threads_Yield();
+
+	if( !gbFDD_IrqFired ) {
+		Log_Warning("FDD", "FDD_WaitIRQ - Timeout");
+		return 1;
+	}	
+
 	gbFDD_IrqFired = 0;
+	return 0;
 }
 
-void FDD_SensInt(int base, Uint8 *sr0, Uint8 *cyl)
+void FDD_SenseInt(int base, Uint8 *sr0, Uint8 *cyl)
 {
-	FDD_int_SendByte(base, CHECK_INTERRUPT_STATUS);
-	if(sr0)	*sr0 = FDD_int_GetByte(base);
-	else	FDD_int_GetByte(base);
-	if(cyl)	*cyl = FDD_int_GetByte(base);
-	else	FDD_int_GetByte(base);
+	FDD_int_SendByte(base, CMD_SENSE_INTERRUPT);
+	FDD_int_GetByte(base, sr0);
+	FDD_int_GetByte(base, cyl);
 }
 
 /**
  * void FDD_int_SendByte(int base, char byte)
  * \brief Sends a command to the controller
  */
-void FDD_int_SendByte(int base, char byte)
+int FDD_int_SendByte(int base, Uint8 byte)
 {
-	volatile int state;
-	int timeout = 128;
-	for( ; timeout--; )
-	{
-	    state = inb(base + PORT_MAINSTATUS);
-	    if ((state & 0xC0) == 0x80)
-	    {
-	        outb(base + PORT_DATA, byte);
-	        return;
-	    }
-	    inb(0x80);	//Delay
+	tTime	end = now() + 1000;	// 1s
+	
+	while( (inb(base + PORT_MAINSTATUS) & 0x80) != 0x80 && now() < end )
+		Threads_Yield();	//Delay
+//		Time_Delay(10);	//Delay
+
+	if( inb(base + PORT_MAINSTATUS) & 0x40 ) {
+		Log_Warning("FDD", "FDD_int_SendByte: DIO set, is this ok?");
+		return -2;
 	}
 	
-	#if WARN
-	Warning("FDD_int_SendByte - Timeout sending byte 0x%x to base 0x%x\n", byte, base);
-	#endif
+	if( now() > end )
+	{
+		Log_Warning("FDD", "FDD_int_SendByte: Timeout sending byte 0x%x to base 0x%x", byte, base);
+		return 1;
+	}
+	outb(base + PORT_DATA, byte);
+//	Log_Debug("FDD", "FDD_int_SendByte: Sent 0x%02x to 0x%x", byte, base);
+	return 0;
 }
 
 /**
  * int FDD_int_GetByte(int base, char byte)
  * \brief Receive data from fdd controller
  */
-int FDD_int_GetByte(int base)
+int FDD_int_GetByte(int base, Uint8 *value)
 {
-	volatile int state;
-	int timeout;
-	for( timeout = 128; timeout--; )
+	tTime	end = now() + 1000;	// 1s
+	
+	while( (inb(base + PORT_MAINSTATUS) & 0x80) != 0x80 && now() < end )
+		Time_Delay(10);
+	
+	if( !(inb(base + PORT_MAINSTATUS) & 0x40) ) {
+		Log_Warning("FDD", "FDD_int_GetByte: DIO unset, is this ok?");
+		return -2;
+	}
+
+	if( now() > end )
 	{
-	    state = inb((base + PORT_MAINSTATUS));
-	    if ((state & 0xd0) == 0xd0)
-		    return inb(base + PORT_DATA);
-	    inb(0x80);
+		Log_Warning("FDD", "FDD_int_GetByte: Timeout reading byte from base 0x%x", base);
+		return -1;
 	}
-	return -1;
+	
+	if(value)
+		*value = inb(base + PORT_DATA);
+	else
+		inb(base + PORT_DATA);
+	return 0;
 }
 
 /**
@@ -721,51 +805,80 @@ void FDD_Recalibrate(int disk)
 	LOG("Starting Motor");
 	FDD_int_StartMotor(disk);
 	// Wait for Spinup
-	while(gFDD_Devices[disk].motorState == 1)	Threads_Yield();
+	while(gFDD_Devices[disk].motorState <= 1)	Threads_Yield();
 	
 	LOG("Sending Calibrate Command");
-	FDD_int_SendByte(cPORTBASE[disk>>1], CALIBRATE_DRIVE);
+	FDD_int_SendByte(cPORTBASE[disk>>1], CMD_RECALIBRATE);
 	FDD_int_SendByte(cPORTBASE[disk>>1], disk&1);
 	
 	LOG("Waiting for IRQ");
 	FDD_WaitIRQ();
-	FDD_SensInt(cPORTBASE[disk>>1], NULL, NULL);
+	FDD_SenseInt(cPORTBASE[disk>>1], NULL, NULL);
 	
 	LOG("Stopping Motor");
-	gFDD_Devices[disk].timer = Time_CreateTimer(MOTOR_OFF_DELAY, FDD_int_StopMotor, (void*)(Uint)disk);
+	FDD_int_StopMotor(disk);
 	LEAVE('-');
 }
 
+/**
+ * \brief Reconfigure the controller
+ */
+int FDD_Reconfigure(int ID)
+{
+	Uint16	base = cPORTBASE[ID];
+	
+	ENTER("iID", ID);
+	
+	FDD_int_SendByte(base, CMD_CONFIGURE);
+	FDD_int_SendByte(base, 0);
+	// Implied seek enabled, FIFO Enabled, Drive Polling Disabled, data buffer threshold 8 bytes
+	FDD_int_SendByte(base, (1 << 6) | (0 << 5) | (0 << 4) | 7);
+	FDD_int_SendByte(base, 0);	// Precompensation - use default
+	
+	// Commit
+	FDD_int_SendByte(base, CMD_LOCK|CMD_FLAG_MULTI_TRACK);
+	FDD_int_GetByte(base, NULL);
+	
+	LEAVE('i', 0);
+	return 0;
+}
+
 /**
  * \brief Reset the specified FDD controller
  */
-void FDD_Reset(int id)
+int FDD_Reset(int id)
 {
-	int base = cPORTBASE[id];
+	Uint16	base = cPORTBASE[id];
+	Uint8	motor_state;
 	
 	ENTER("iID", id);
+
+	// Reset the card
+	motor_state = inb(base + PORT_DIGOUTPUT) & 0xF0;
+	outb(base + PORT_DIGOUTPUT, motor_state|0);	// Disable FDC
+	Time_Delay(1);
+	outb(base + PORT_DIGOUTPUT, motor_state|8|4);	// Re-enable FDC (DMA and Enable)
 	
-	outb(base + PORT_DIGOUTPUT, 0);	// Stop Motors & Disable FDC
-	outb(base + PORT_DIGOUTPUT, 0x0C);	// Re-enable FDC (DMA and Enable)
-	
+	// Set the data rate
+	outb(base + PORT_DATARATE, 0);	// Set data rate to 500K/s
+
+	// Wait for IRQ
 	LOG("Awaiting IRQ");
 	
 	FDD_WaitIRQ();
-	FDD_SensInt(base, NULL, NULL);
+
+	FDD_SenseInt(base, NULL, NULL);
 	
-	LOG("Setting Driver Info");
-	outb(base + PORT_DATARATE, 0);	// Set data rate to 500K/s
-	FDD_int_SendByte(base, FIX_DRIVE_DATA);	// Step and Head Load Times
+	// Specify
+	FDD_int_SendByte(base, CMD_SPECIFY);	// Step and Head Load Times
 	FDD_int_SendByte(base, 0xDF);	// Step Rate Time, Head Unload Time (Nibble each)
 	FDD_int_SendByte(base, 0x02);	// Head Load Time >> 1
-	while(FDD_int_SeekTrack(0, 0, 1) == 0);	// set track
-	while(FDD_int_SeekTrack(0, 1, 1) == 0);	// set track
-	
+
 	LOG("Recalibrating Disk");
 	FDD_Recalibrate((id<<1)|0);
 	FDD_Recalibrate((id<<1)|1);
 
-	LEAVE('-');
+	LEAVE_RET('i', 0);
 }
 
 /**
@@ -790,26 +903,55 @@ void FDD_int_TimerCallback(void *Arg)
 void FDD_int_StartMotor(int disk)
 {
 	Uint8	state;
+	if( gFDD_Devices[disk].motorState != 0 )	return ;
+	// Set motor ON bit
 	state = inb( cPORTBASE[ disk>>1 ] + PORT_DIGOUTPUT );
 	state |= 1 << (4+disk);
 	outb( cPORTBASE[ disk>>1 ] + PORT_DIGOUTPUT, state );
+	// Mark as spinning up
 	gFDD_Devices[disk].motorState = 1;
+	// Schedule a timer for when it's up to speed
 	gFDD_Devices[disk].timer = Time_CreateTimer(MOTOR_ON_DELAY, FDD_int_TimerCallback, (void*)(Uint)disk);
 }
 
 /**
- * \fn void FDD_int_StopMotor(int disk)
+ * \brief Schedule the drive motor to stop
+ * \param Disk	Disk number to stop
+ */
+void FDD_int_StopMotor(int Disk)
+{
+	// Ignore if the motor is aready off
+	if( gFDD_Devices[Disk].motorState == 0 )	return ;
+	
+	// Don't double-schedule timer
+	if( gFDD_Devices[Disk].timer != -1 )
+	{
+		Time_RemoveTimer( gFDD_Devices[Disk].timer );
+	}
+	
+	gFDD_Devices[Disk].timer = Time_CreateTimer(MOTOR_OFF_DELAY, FDD_int_StopMotorCallback, (void*)(Uint)Disk);
+}
+
+/**
  * \brief Stops FDD Motor
  */
-void FDD_int_StopMotor(void *Arg)
+void FDD_int_StopMotorCallback(void *Arg)
 {
 	Uint8	state, disk = (Uint)Arg;
-	if( IS_LOCKED(&glFDD) )	return ;
+
+	// Mutex is only locked if disk is in use
+	if( Mutex_IsLocked(&glFDD) )	return ;
+
 	ENTER("iDisk", disk);
-	
+
+	// Clear motor on bit	
 	state = inb( cPORTBASE[ disk>>1 ] + PORT_DIGOUTPUT );
 	state &= ~( 1 << (4+disk) );
 	outb( cPORTBASE[ disk>>1 ] + PORT_DIGOUTPUT, state );
+	
+	// Mark as off
 	gFDD_Devices[disk].motorState = 0;
+
 	LEAVE('-');
 }
+