#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>
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 ===
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];
tMutex glFDD;
volatile int gbFDD_IrqFired = 0;
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++)
{
}
}
- 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
void FDD_UnloadModule()
{
int i;
- //DevFS_DelDevice( &gFDD_DriverInfo );
+ 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);
}
Mutex_Release(&glFDD);
//IRQ_Clear(6);
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);
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);
- // 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
-
// Read Data from DMA
LOG("Setting DMA for read");
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);
}
// 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);
+ 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; }
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; }
if(st1 & 0x02) {
LOG("Floppy not writable");
+ // Return error without triggering the attempt count check
i = FDD_MAX_READWRITE_ATTEMPTS+1;
break;
}
// Success!
break;
}
+ #undef SENDB
// Release Spinlock
LOG("Realeasing Spinlock and setting motor to stop");
}
// 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 ) {
* \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;
*/
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];
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;
}
* \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;
}
/**
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);
}
/**
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('-');
}
+