From: John Hodge <tpg@mutabah.net>
Date: Sun, 9 Oct 2011 04:19:17 +0000 (+0800)
Subject: Modules/FDD - Heaps of changes when debugging, may be broken now
X-Git-Tag: rel0.11~6
X-Git-Url: https://git.ucc.asn.au/?a=commitdiff_plain;h=e3554b0be0e13cf461b27132187172322d1f3ac3;p=tpg%2Facess2.git

Modules/FDD - Heaps of changes when debugging, may be broken now
---

diff --git a/Modules/Storage/FDD/fdd.c b/Modules/Storage/FDD/fdd.c
index e507e94e..9cf808d2 100644
--- a/Modules/Storage/FDD/fdd.c
+++ b/Modules/Storage/FDD/fdd.c
@@ -62,7 +62,7 @@ enum FloppyPorts {
 	PORT_DIGOUTPUT	= 0x2,
 	PORT_MAINSTATUS	= 0x4,
 	PORT_DATARATE	= 0x4,
-	PORT_DATA		= 0x5,
+	PORT_DATA   	= 0x5,
 	PORT_DIGINPUT	= 0x7,
 	PORT_CONFIGCTRL	= 0x7
 };
@@ -107,20 +107,24 @@ 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
+ int	FDD_WaitIRQ();
 void	FDD_IRQHandler(int Num, void *Ptr);
-inline void	FDD_WaitIRQ();
-void	FDD_SensInt(int base, Uint8 *sr0, Uint8 *cyl);
- int	FDD_int_SendByte(int base, Uint8 Byte);
- int	FDD_int_GetByte(int base, Uint8 *Byte);
+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, "x86_ISADMA", NULL);
 t_floppyDevice	gFDD_Devices[2];
@@ -157,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++)
 		{
@@ -165,12 +176,6 @@ 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;
-	}
-	
 	// Install IRQ6 Handler
 	IRQ_AddHandler(6, FDD_IRQHandler, NULL);
 
@@ -192,6 +197,24 @@ int FDD_Install(char **Arguments)
 	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
@@ -243,7 +266,7 @@ void FDD_UnloadModule()
 	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);
 	}
 	Mutex_Release(&glFDD);
 	//IRQ_Clear(6);
@@ -395,66 +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);
-	
+
+	// 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);
 	Mutex_Release(&glFDD);
 	
-	LOG("Wait for the motor to spin up");
-	
 	// Wait for spinup
+	LOG("Wait for the motor to spin up");
 	while(gFDD_Devices[Disk].motorState == 1)	Threads_Yield();
 	
 	LOG("Acquire Spinlock");
 	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 ) {
-		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/Write 512 Bytes from channel 2
 	
 	LOG("Sending command");
 	
+	#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, CMD_WRITE_DATA|CMD_FLAG_MFM_ENCODING);
-		else
-			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);
-		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);
 		}
@@ -510,6 +524,7 @@ 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");
@@ -523,7 +538,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);
+	FDD_int_StopMotor(Disk);
 
 	// Error check
 	if( i < FDD_MAX_READWRITE_ATTEMPTS ) {
@@ -540,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;
@@ -579,13 +594,12 @@ int FDD_WriteSector(Uint32 Disk, Uint64 LBA, void *Buffer)
 }
 
 /**
- * \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=0, cyl=0;
-	 int	base;
+	 int	base, i, bUnclean;
 	
 	base = cPORTBASE[disk>>1];
 	
@@ -594,17 +608,33 @@ int FDD_int_SeekTrack(int disk, int head, int track)
 		return 1;
 	
 	// - Seek Head 0
-	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();
-	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;
 
@@ -682,17 +712,27 @@ void FDD_IRQHandler(int Num, void *Ptr)
 }
 
 /**
- * \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, CMD_SENSE_INTERRUPT);
 	FDD_int_GetByte(base, sr0);
@@ -709,23 +749,21 @@ int FDD_int_SendByte(int base, Uint8 byte)
 	
 	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( now() < end )
-	{
-		outb(base + PORT_DATA, byte);
-//		Log_Debug("FDD", "FDD_int_SendByte: Sent 0x%02x to 0x%x", byte, base);
-		return 0;
-	}
-	else
+	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;
 }
 
 /**
@@ -737,25 +775,24 @@ int FDD_int_GetByte(int base, Uint8 *value)
 	tTime	end = now() + 1000;	// 1s
 	
 	while( (inb(base + PORT_MAINSTATUS) & 0x80) != 0x80 && now() < end )
-		Threads_Yield();
+		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 )
-	{
-		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
+	if( now() > end )
 	{
 		Log_Warning("FDD", "FDD_int_GetByte: Timeout reading byte from base 0x%x", base);
 		return -1;
 	}
+	
+	if(value)
+		*value = inb(base + PORT_DATA);
+	else
+		inb(base + PORT_DATA);
+	return 0;
 }
 
 /**
@@ -768,7 +805,7 @@ 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], CMD_RECALIBRATE);
@@ -776,10 +813,10 @@ void FDD_Recalibrate(int disk)
 	
 	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('-');
 }
 
@@ -812,16 +849,15 @@ int FDD_Reconfigure(int ID)
 int FDD_Reset(int id)
 {
 	Uint16	base = cPORTBASE[id];
-	 int	retries;
+	Uint8	motor_state;
 	
 	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)
+	// 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)
 	
 	// Set the data rate
 	outb(base + PORT_DATARATE, 0);	// Set data rate to 500K/s
@@ -830,29 +866,14 @@ int FDD_Reset(int id)
 	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);
+
+	FDD_SenseInt(base, NULL, NULL);
 	
 	// 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
 
-	// 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);
-	
 	LOG("Recalibrating Disk");
 	FDD_Recalibrate((id<<1)|0);
 	FDD_Recalibrate((id<<1)|1);
@@ -882,27 +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;
+
+	// 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('-');
 }