X-Git-Url: https://git.ucc.asn.au/?a=blobdiff_plain;f=Modules%2FStorage%2FFDD%2Ffdd.c;h=5c2848ffee5a3ded0230e7e8761906cee2641c67;hb=209b173fbcd7bcadd5b3546a2f773312d668c5a1;hp=cc75bfcd669c04f46c224166acc102c707e1d8ce;hpb=83612bb37fbd8e84d90ecb9e6a7157aadd1e2175;p=tpg%2Facess2.git

diff --git a/Modules/Storage/FDD/fdd.c b/Modules/Storage/FDD/fdd.c
index cc75bfcd..5c2848ff 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>
 
@@ -67,19 +67,29 @@ enum FloppyPorts {
 	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 ===
@@ -88,7 +98,7 @@ enum FloppyCommands {
 void	FDD_UnloadModule();
 // --- VFS Methods
 char	*FDD_ReadDir(tVFS_Node *Node, int pos);
-tVFS_Node	*FDD_FindDir(tVFS_Node *dirNode, char *Name);
+tVFS_Node	*FDD_FindDir(tVFS_Node *dirNode, const char *Name);
  int	FDD_IOCtl(tVFS_Node *Node, int ID, void *Data);
 Uint64	FDD_ReadFS(tVFS_Node *node, Uint64 off, Uint64 len, void *buffer);
 // --- Functions for IOCache/DrvUtil
@@ -97,13 +107,14 @@ 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);
+void	FDD_IRQHandler(int Num, void *Ptr);
 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_int_SendByte(int base, Uint8 Byte);
+ int	FDD_int_GetByte(int base, Uint8 *Byte);
+ 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);
@@ -111,9 +122,9 @@ void	FDD_int_StartMotor(int Disk);
  int	FDD_int_GetDims(int type, int lba, int *c, int *h, int *s, int *spt);
 
 // === 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",
@@ -160,12 +171,27 @@ int FDD_Install(char **Arguments)
 		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;
+	}
 	
 	// Initialise Root Node
 	gFDD_DriverInfo.RootNode.CTime = gFDD_DriverInfo.RootNode.MTime
@@ -213,13 +239,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);
 	}
-	RELEASE(&glFDD);
+	Mutex_Release(&glFDD);
 	//IRQ_Clear(6);
 }
 
@@ -227,7 +253,7 @@ void FDD_UnloadModule()
  * \fn char *FDD_ReadDir(tVFS_Node *Node, int pos)
  * \brief Read Directory
  */
-char *FDD_ReadDir(tVFS_Node *Node, int Pos)
+char *FDD_ReadDir(tVFS_Node *UNUSED(Node), int Pos)
 {
 	char	name[2] = "0\0";
 
@@ -241,10 +267,10 @@ char *FDD_ReadDir(tVFS_Node *Node, int Pos)
 }
 
 /**
- * \fn tVFS_Node *FDD_FindDir(tVFS_Node *Node, char *filename);
+ * \fn tVFS_Node *FDD_FindDir(tVFS_Node *Node, const char *filename);
  * \brief Find File Routine (for vfs_node)
  */
-tVFS_Node *FDD_FindDir(tVFS_Node *Node, char *Filename)
+tVFS_Node *FDD_FindDir(tVFS_Node *UNUSED(Node), const char *Filename)
 {
 	 int	i;
 	
@@ -287,14 +313,11 @@ static const char	*casIOCTLS[] = {DRV_IOCTLNAMES,DRV_DISK_IOCTLNAMES,NULL};
  * \fn int FDD_IOCtl(tVFS_Node *Node, int id, void *data)
  * \brief Stub ioctl function
  */
-int FDD_IOCtl(tVFS_Node *Node, int ID, void *Data)
+int FDD_IOCtl(tVFS_Node *UNUSED(Node), int ID, void *Data)
 {
 	switch(ID)
 	{
-	case DRV_IOCTL_TYPE:	return DRV_TYPE_DISK;
-	case DRV_IOCTL_IDENT:	return ModUtil_SetIdent(Data, "FDD");
-	case DRV_IOCTL_VERSION:	return FDD_VERSION;
-	case DRV_IOCTL_LOOKUP:	return ModUtil_LookupString((char**)casIOCTLS, Data);
+	BASE_IOCTLS(DRV_TYPE_DISK, "FDD", FDD_VERSION, casIOCTLS);
 	
 	case DISK_IOCTL_GETBLOCKSIZE:	return 512;	
 	
@@ -358,7 +381,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);
 	
@@ -373,11 +396,11 @@ int FDD_int_ReadWriteSector(Uint32 Disk, Uint64 SectorAddr, int Write, void *Buf
 	}
 	LOG("Cyl=%i, Head=%i, Sector=%i", cyl, head, sec);
 	
-	LOCK(&glFDD);	// Lock to stop the motor stopping on us
+	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);
+	Mutex_Release(&glFDD);
 	
 	LOG("Wait for the motor to spin up");
 	
@@ -385,35 +408,34 @@ int FDD_int_ReadWriteSector(Uint32 Disk, Uint64 SectorAddr, int Write, void *Buf
 	while(gFDD_Devices[Disk].motorState == 1)	Threads_Yield();
 	
 	LOG("Acquire Spinlock");
-	LOCK(&glFDD);
+	Mutex_Acquire(&glFDD);
 	
+	#if 0
 	// 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);
+		Mutex_Release(&glFDD);
 		LEAVE('i', 0);
 		return 0;
 	}
 	//FDD_SensInt(base, NULL, NULL);	// Wait for IRQ
+	#endif
 		
 	// 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);
-	
 	for( i = 0; i < FDD_MAX_READWRITE_ATTEMPTS; i ++ )
 	{
 		if( Write )
-			FDD_int_SendByte(base, READ_SECTOR);	// Was 0xE6
+			FDD_int_SendByte(base, CMD_WRITE_DATA|CMD_FLAG_MFM_ENCODING);
 		else
-			FDD_int_SendByte(base, READ_SECTOR);	// Was 0xE6
+			FDD_int_SendByte(base, CMD_READ_DATA|CMD_FLAG_MFM_ENCODING);
 		FDD_int_SendByte(base, (head << 2) | (Disk&1));
 		FDD_int_SendByte(base, (Uint8)cyl);
 		FDD_int_SendByte(base, (Uint8)head);
@@ -440,26 +462,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;	}
@@ -467,7 +489,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;	}
@@ -480,6 +502,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;
 		}
@@ -490,7 +513,7 @@ int FDD_int_ReadWriteSector(Uint32 Disk, Uint64 SectorAddr, int Write, void *Buf
 	
 	// 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",
@@ -502,6 +525,7 @@ 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);
 
+	// Error check
 	if( i < FDD_MAX_READWRITE_ATTEMPTS ) {
 		LEAVE('i', 0);
 		return 0;
@@ -550,7 +574,7 @@ 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;
 }
 
@@ -560,7 +584,7 @@ int FDD_WriteSector(Uint32 Disk, Uint64 LBA, void *Buffer)
  */
 int FDD_int_SeekTrack(int disk, int head, int track)
 {
-	Uint8	sr0, cyl;
+	Uint8	sr0=0, cyl=0;
 	 int	base;
 	
 	base = cPORTBASE[disk>>1];
@@ -570,7 +594,7 @@ 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, CMD_SEEK_TRACK);
 	FDD_int_SendByte(base, (head<<2)|(disk&1));
 	FDD_int_SendByte(base, track);	// Send Seek command
 	FDD_WaitIRQ();
@@ -583,6 +607,11 @@ int FDD_int_SeekTrack(int disk, int head, int track)
 	
 	// Set Track in structure
 	gFDD_Devices[disk].track[head] = track;
+
+	LOG("Time_Delay(100)");	
+	// Wait for Head to settle
+	Time_Delay(100);
+	
 	return 1;
 }
 
@@ -647,7 +676,7 @@ 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;
 }
@@ -665,53 +694,66 @@ inline void FDD_WaitIRQ()
 
 void FDD_SensInt(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
+
+	if( inb(base + PORT_MAINSTATUS) & 0x40 ) {
+		Log_Warning("FDD", "FDD_int_SendByte: DIO set, is this ok?");
 	}
 	
-	#if WARN
-	Warning("FDD_int_SendByte - Timeout sending byte 0x%x to base 0x%x\n", byte, base);
-	#endif
+	if( now() < end )
+	{
+		outb(base + PORT_DATA, byte);
+//		Log_Debug("FDD", "FDD_int_SendByte: Sent 0x%02x to 0x%x", byte, base);
+		return 0;
+	}
+	else
+	{
+		Log_Warning("FDD", "FDD_int_SendByte: Timeout sending byte 0x%x to base 0x%x", byte, base);
+		return 1;
+	}
 }
 
 /**
  * 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 )
+		Threads_Yield();
+	
+	if( !(inb(base + PORT_MAINSTATUS) & 0x40) ) {
+		Log_Warning("FDD", "FDD_int_GetByte: DIO unset, is this ok?");
+	}
+
+	if( now() < end )
 	{
-	    state = inb((base + PORT_MAINSTATUS));
-	    if ((state & 0xd0) == 0xd0)
-		    return inb(base + PORT_DATA);
-	    inb(0x80);
+		Uint8	tmp = inb(base + PORT_DATA);
+		if(value)	*value = tmp;
+//		Log_Debug("FDD", "FDD_int_GetByte: Read 0x%02x from 0x%x", *value, base);
+		return 0;
+	}
+	else
+	{
+		Log_Warning("FDD", "FDD_int_GetByte: Timeout reading byte from base 0x%x", base);
+		return -1;
 	}
-	return -1;
 }
 
 /**
@@ -727,7 +769,7 @@ void FDD_Recalibrate(int disk)
 	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");
@@ -739,36 +781,79 @@ void FDD_Recalibrate(int 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];
+	 int	retries;
 	
 	ENTER("iID", id);
+
+	// Reset the card	
+	outb(base + PORT_DIGOUTPUT, 0);	// Disable FDC
+	// Wait 4 microseconds - or use 1 thread delay
+	Threads_Yield();
+	Threads_Yield();
+	outb(base + PORT_DIGOUTPUT, 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();
+	LOG("4x SenseInterrupt");
+	FDD_SensInt(base, NULL, NULL);
+	FDD_SensInt(base, NULL, NULL);
+	FDD_SensInt(base, NULL, NULL);
 	FDD_SensInt(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
+
+	// Recalibrate disks
+	LOG("Recalibrate disks (16x seek)");
+	retries = 16;
+	while(FDD_int_SeekTrack(0, 0, 1) == 0 && retries --);	// set track
+	if(retries < 0)	LEAVE_RET('i', -1);
+
+	retries = 16;
+	while(FDD_int_SeekTrack(0, 1, 1) == 0 && retries --);	// set track
+	if(retries < 0)	LEAVE_RET('i', -1);
 	
 	LOG("Recalibrating Disk");
 	FDD_Recalibrate((id<<1)|0);
 	FDD_Recalibrate((id<<1)|1);
 
-	LEAVE('-');
+	LEAVE_RET('i', 0);
 }
 
 /**
@@ -807,7 +892,7 @@ void FDD_int_StartMotor(int disk)
 void FDD_int_StopMotor(void *Arg)
 {
 	Uint8	state, disk = (Uint)Arg;
-	if( IS_LOCKED(&glFDD) )	return ;
+	if( Mutex_IsLocked(&glFDD) )	return ;
 	ENTER("iDisk", disk);
 	
 	state = inb( cPORTBASE[ disk>>1 ] + PORT_DIGOUTPUT );
@@ -816,3 +901,4 @@ void FDD_int_StopMotor(void *Arg)
 	gFDD_Devices[disk].motorState = 0;
 	LEAVE('-');
 }
+