3 * Floppy Disk Access Code
9 #include <api_drv_disk.h>
16 // --- Current Version
17 #define FDD_VERSION ((0<<8)|(75))
20 #define FDD_SEEK_TIMEOUT 10 // Timeout for a seek operation
21 #define MOTOR_ON_DELAY 500 // Miliseconds
22 #define MOTOR_OFF_DELAY 2000 // Miliseconds
23 #define FDD_MAX_READWRITE_ATTEMPTS 16
27 * \brief Representation of a floppy drive
29 typedef struct sFloppyDrive
32 volatile int motorState; //2 - On, 1 - Spinup, 0 - Off
37 tIOCache *CacheHandle;
42 * \brief Cached Sector
47 Uint16 sector; // Allows 32Mb of addressable space (Plenty for FDD)
52 static const char *cFDD_TYPES[] = {"None", "360kB 5.25\"", "1.2MB 5.25\"", "720kB 3.5\"", "1.44MB 3.5\"", "2.88MB 3.5\"" };
53 static const int cFDD_SIZES[] = { 0, 360*1024, 1200*1024, 720*1024, 1440*1024, 2880*1024 };
54 static const short cPORTBASE[] = { 0x3F0, 0x370 };
56 static const char *cFDD_STATUSES[] = {NULL, "Error", "Invalid command", "Drive not ready"};
63 PORT_MAINSTATUS = 0x4,
70 #define CMD_FLAG_MULTI_TRACK 0x80 //!< Multitrack
71 #define CMD_FLAG_MFM_ENCODING 0x40 //!< MFM Encoding Mode (Always set for read/write/format/verify)
72 #define CMD_FLAG_SKIP_MODE 0x20 //!< Skip Mode (don't use)
74 CMD_READ_TRACK = 0x02,
76 CMD_SENSE_STATUS = 0x04,
77 CMD_WRITE_DATA = 0x05,
79 CMD_RECALIBRATE = 0x07,
80 CMD_SENSE_INTERRUPT = 0x08,
81 CMD_WRITE_DEL_DATA = 0x09,
82 CMD_READ_SECTOR_ID = 0x0A,
84 CMD_READ_DEL_DATA = 0x0C,
85 CMD_FORMAT_TRACK = 0x0D,
87 CMD_SEEK_TRACK = 0x0F,
90 CMD_LOCK = 0x14, //!< Save controller parameters
97 int FDD_Install(char **Arguments);
98 void FDD_UnloadModule();
100 char *FDD_ReadDir(tVFS_Node *Node, int pos);
101 tVFS_Node *FDD_FindDir(tVFS_Node *dirNode, const char *Name);
102 int FDD_IOCtl(tVFS_Node *Node, int ID, void *Data);
103 Uint64 FDD_ReadFS(tVFS_Node *node, Uint64 off, Uint64 len, void *buffer);
104 // --- Functions for IOCache/DrvUtil
105 Uint FDD_ReadSectors(Uint64 SectorAddr, Uint Count, void *Buffer, Uint Disk);
106 // --- Raw Disk Access
107 int FDD_ReadSector(Uint32 disk, Uint64 lba, void *Buffer);
108 int FDD_WriteSector(Uint32 Disk, Uint64 LBA, void *Buffer);
111 void FDD_IRQHandler(int Num, void *Ptr);
112 void FDD_SenseInt(int base, Uint8 *sr0, Uint8 *cyl);
114 int FDD_Reset(int id);
115 void FDD_Recalibrate(int disk);
116 int FDD_Reconfigure(int ID);
118 int FDD_int_SeekTrack(int disk, int head, int track);
119 void FDD_int_TimerCallback(void *Arg);
120 void FDD_int_StopMotor(int Disk);
121 void FDD_int_StopMotorCallback(void *Arg);
122 void FDD_int_StartMotor(int Disk);
123 int FDD_int_GetDims(int type, int lba, int *c, int *h, int *s, int *spt);
125 int FDD_int_SendByte(int base, Uint8 Byte);
126 int FDD_int_GetByte(int base, Uint8 *Byte);
129 MODULE_DEFINE(0, FDD_VERSION, FDD, FDD_Install, NULL, "x86_ISADMA", NULL);
130 t_floppyDevice gFDD_Devices[2];
132 volatile int gbFDD_IrqFired = 0;
133 tDevFS_Driver gFDD_DriverInfo = {
138 .ACLs = &gVFS_ACL_EveryoneRX,
139 .Flags = VFS_FFLAG_DIRECTORY,
140 .ReadDir = FDD_ReadDir,
141 .FindDir = FDD_FindDir,
148 * \fn int FDD_Install(char **Arguments)
149 * \brief Installs floppy driver
151 int FDD_Install(char **Arguments)
154 char **args = Arguments;
156 // Determine Floppy Types (From CMOS)
159 gFDD_Devices[0].type = data >> 4;
160 gFDD_Devices[1].type = data & 0xF;
161 gFDD_Devices[0].track[0] = -1;
162 gFDD_Devices[1].track[1] = -1;
164 Log_Log("FDD", "Detected Disk 0: %s and Disk 1: %s", cFDD_TYPES[data>>4], cFDD_TYPES[data&0xF]);
167 return MODULE_ERR_NOTNEEDED;
174 if(strcmp(*args, "disable")==0)
175 return MODULE_ERR_NOTNEEDED;
179 // Install IRQ6 Handler
180 IRQ_AddHandler(6, FDD_IRQHandler, NULL);
182 // Ensure the FDD version is 0x90
185 FDD_int_SendByte(cPORTBASE[0], CMD_VERSION);
186 FDD_int_GetByte(cPORTBASE[0], &tmp);
188 Log_Error("FDD", "Version(0x%2x) != 0x90", tmp);
189 return MODULE_ERR_NOTNEEDED;
196 // Reset Primary FDD Controller
197 if( FDD_Reset(0) != 0 ) {
198 return MODULE_ERR_MISC;
205 LOG("Recalibrate disks (16x seek)");
207 while(FDD_int_SeekTrack(0, 0, 1) == 0 && retries --)
208 Threads_Yield(); // set track
209 if(retries < 0) LEAVE_RET('i', -1);
212 while(FDD_int_SeekTrack(0, 1, 1) == 0 && retries --)
213 Threads_Yield(); // set track
214 if(retries < 0) LEAVE_RET('i', -1);
219 // Initialise Root Node
220 gFDD_DriverInfo.RootNode.CTime = gFDD_DriverInfo.RootNode.MTime
221 = gFDD_DriverInfo.RootNode.ATime = now();
223 // Initialise Child Nodes
224 gFDD_Devices[0].Node.Inode = 0;
225 gFDD_Devices[0].Node.Flags = 0;
226 gFDD_Devices[0].Node.NumACLs = 0;
227 gFDD_Devices[0].Node.Read = FDD_ReadFS;
228 gFDD_Devices[0].Node.Write = NULL;//FDD_WriteFS;
229 memcpy(&gFDD_Devices[1].Node, &gFDD_Devices[0].Node, sizeof(tVFS_Node));
231 gFDD_Devices[1].Node.Inode = 1;
234 gFDD_Devices[0].Node.Size = cFDD_SIZES[data >> 4];
235 gFDD_Devices[1].Node.Size = cFDD_SIZES[data & 0xF];
237 // Create Sector Cache
238 if( cFDD_SIZES[data >> 4] )
240 gFDD_Devices[0].CacheHandle = IOCache_Create(
241 FDD_WriteSector, 0, 512,
242 gFDD_Devices[0].Node.Size / (512*4)
243 ); // Cache is 1/4 the size of the disk
245 if( cFDD_SIZES[data & 15] )
247 gFDD_Devices[1].CacheHandle = IOCache_Create(
248 FDD_WriteSector, 0, 512,
249 gFDD_Devices[1].Node.Size / (512*4)
250 ); // Cache is 1/4 the size of the disk
253 // Register with devfs
254 DevFS_AddDevice(&gFDD_DriverInfo);
256 return MODULE_ERR_OK;
260 * \brief Prepare the module for removal
262 void FDD_UnloadModule()
265 DevFS_DelDevice( &gFDD_DriverInfo );
266 Mutex_Acquire(&glFDD);
268 Time_RemoveTimer(gFDD_Devices[i].timer);
269 FDD_int_StopMotor(i);
271 Mutex_Release(&glFDD);
276 * \fn char *FDD_ReadDir(tVFS_Node *Node, int pos)
277 * \brief Read Directory
279 char *FDD_ReadDir(tVFS_Node *UNUSED(Node), int Pos)
281 char name[2] = "0\0";
283 if(Pos >= 2 || Pos < 0) return NULL;
285 if(gFDD_Devices[Pos].type == 0) return VFS_SKIP;
293 * \fn tVFS_Node *FDD_FindDir(tVFS_Node *Node, const char *filename);
294 * \brief Find File Routine (for vfs_node)
296 tVFS_Node *FDD_FindDir(tVFS_Node *UNUSED(Node), const char *Filename)
300 ENTER("sFilename", Filename);
302 // Sanity check string
303 if(Filename == NULL) {
308 // Check string length (should be 1)
309 if(Filename[0] == '\0' || Filename[1] != '\0') {
314 // Get First character
315 i = Filename[0] - '0';
317 // Check for 1st disk and if it is present return
318 if(i == 0 && gFDD_Devices[0].type != 0) {
319 LEAVE('p', &gFDD_Devices[0].Node);
320 return &gFDD_Devices[0].Node;
323 // Check for 2nd disk and if it is present return
324 if(i == 1 && gFDD_Devices[1].type != 0) {
325 LEAVE('p', &gFDD_Devices[1].Node);
326 return &gFDD_Devices[1].Node;
334 static const char *casIOCTLS[] = {DRV_IOCTLNAMES,DRV_DISK_IOCTLNAMES,NULL};
336 * \fn int FDD_IOCtl(tVFS_Node *Node, int id, void *data)
337 * \brief Stub ioctl function
339 int FDD_IOCtl(tVFS_Node *UNUSED(Node), int ID, void *Data)
343 BASE_IOCTLS(DRV_TYPE_DISK, "FDD", FDD_VERSION, casIOCTLS);
345 case DISK_IOCTL_GETBLOCKSIZE: return 512;
353 * \fn Uint64 FDD_ReadFS(tVFS_Node *Node, Uint64 Offset, Uint64 Length, void *Buffer)
354 * \brief Read Data from a disk
356 Uint64 FDD_ReadFS(tVFS_Node *Node, Uint64 Offset, Uint64 Length, void *Buffer)
360 ENTER("pNode XOffset XLength pBuffer", Node, Offset, Length, Buffer);
367 if(Node->Inode != 0 && Node->Inode != 1) {
372 ret = DrvUtil_ReadBlock(Offset, Length, Buffer, FDD_ReadSectors, 512, Node->Inode);
378 * \brief Reads \a Count contiguous sectors from a disk
379 * \param SectorAddr Address of the first sector
380 * \param Count Number of sectors to read
381 * \param Buffer Destination Buffer
382 * \param Disk Disk Number
383 * \return Number of sectors read
384 * \note Used as a ::DrvUtil_ReadBlock helper
386 Uint FDD_ReadSectors(Uint64 SectorAddr, Uint Count, void *Buffer, Uint Disk)
391 if( FDD_ReadSector(Disk, SectorAddr, Buffer) != 1 )
394 Buffer = (void*)( (tVAddr)Buffer + 512 );
401 int FDD_int_ReadWriteSector(Uint32 Disk, Uint64 SectorAddr, int Write, void *Buffer)
406 int lba = SectorAddr;
407 Uint8 st0=0, st1=0, st2=0, bps=0; // Status Values
409 ENTER("iDisk XSectorAddr pBuffer", Disk, SectorAddr, Buffer);
411 base = cPORTBASE[Disk >> 1];
413 LOG("Calculating Disk Dimensions");
415 if(FDD_int_GetDims(gFDD_Devices[Disk].type, lba, &cyl, &head, &sec, &spt) != 1)
420 LOG("Cyl=%i, Head=%i, Sector=%i", cyl, head, sec);
423 Mutex_Acquire(&glFDD); // Lock to stop the motor stopping on us
424 Time_RemoveTimer(gFDD_Devices[Disk].timer); // Remove Old Timer
425 // Start motor if needed
426 if(gFDD_Devices[Disk].motorState != 2) FDD_int_StartMotor(Disk);
427 Mutex_Release(&glFDD);
430 LOG("Wait for the motor to spin up");
431 while(gFDD_Devices[Disk].motorState == 1) Threads_Yield();
433 LOG("Acquire Spinlock");
434 Mutex_Acquire(&glFDD);
436 // Read Data from DMA
437 LOG("Setting DMA for read");
438 DMA_SetChannel(2, 512, !Write); // Read/Write 512 Bytes from channel 2
440 LOG("Sending command");
442 #define SENDB(__data) if(FDD_int_SendByte(base, __data)) { FDD_Reset(Disk >> 1); continue; }
444 for( i = 0; i < FDD_MAX_READWRITE_ATTEMPTS; i ++ )
446 FDD_int_SeekTrack(Disk, head, cyl);
448 SENDB(CMD_WRITE_DATA|CMD_FLAG_MFM_ENCODING);
451 SENDB(CMD_READ_DATA|CMD_FLAG_MFM_ENCODING);
453 SENDB( (head << 2) | (Disk&1) );
457 SENDB(0x02); // Bytes Per Sector (Real BPS=128*2^{val})
459 SENDB(0x1B); // Gap Length (27 is default)
460 SENDB(0xFF); // Data Length
465 DMA_WriteData(2, 512, Buffer);
466 LOG("Waiting for Data to be written");
467 if( FDD_WaitIRQ() ) { FDD_Reset(Disk>>1); continue; }
470 LOG("Waiting for data to be read");
471 if( FDD_WaitIRQ() ) { FDD_Reset(Disk>>1); continue; }
473 DMA_ReadData(2, 512, Buffer);
476 // Clear Input Buffer
477 LOG("Clearing Input Buffer");
479 FDD_int_GetByte(base, &st0);
480 FDD_int_GetByte(base, &st1);
481 FDD_int_GetByte(base, &st2);
483 // Cylinder, Head and Sector (mutilated in some way)
484 FDD_int_GetByte(base, NULL); // Cylinder
485 FDD_int_GetByte(base, NULL); // Head
486 FDD_int_GetByte(base, NULL); // Sector
487 // Should be the BPS set above (0x02)
488 FDD_int_GetByte(base, &bps);
493 LOG("Error (st0 & 0xC0) \"%s\"", cFDD_STATUSES[st0 >> 6]);
497 if(st0 & 0x08) { LOG("Drive not ready"); continue; }
498 if(st1 & 0x80) { LOG("End of Cylinder"); continue; }
499 if(st1 & 0x20) { LOG("CRC Error"); continue; }
500 if(st1 & 0x10) { LOG("Controller Timeout"); continue; }
501 if(st1 & 0x04) { LOG("No Data Found"); continue; }
502 if(st1 & 0x01 || st2 & 0x01) {
503 LOG("No Address mark found");
506 if(st2 & 0x40) { LOG("Deleted address mark"); continue; }
507 if(st2 & 0x20) { LOG("CRC error in data"); continue; }
508 if(st2 & 0x10) { LOG("Wrong Cylinder"); continue; }
509 if(st2 & 0x04) { LOG("uPD765 sector not found"); continue; }
510 if(st2 & 0x02) { LOG("Bad Cylinder"); continue; }
513 LOG("Returned BPS = 0x%02x, not 0x02", bps);
518 LOG("Floppy not writable");
519 // Return error without triggering the attempt count check
520 i = FDD_MAX_READWRITE_ATTEMPTS+1;
530 LOG("Realeasing Spinlock and setting motor to stop");
531 Mutex_Release(&glFDD);
533 if(i == FDD_MAX_READWRITE_ATTEMPTS) {
534 Log_Warning("FDD", "Exceeded %i attempts in %s the disk",
535 FDD_MAX_READWRITE_ATTEMPTS,
536 (Write ? "writing to" : "reading from")
540 // Don't turn the motor off now, wait for a while
541 FDD_int_StopMotor(Disk);
544 if( i < FDD_MAX_READWRITE_ATTEMPTS ) {
555 * \fn int FDD_ReadSector(Uint32 Disk, Uint64 SectorAddr, void *Buffer)
556 * \brief Read a sector from disk
557 * \todo Make real-hardware safe (account for read errors)
559 int FDD_ReadSector(Uint32 Disk, Uint64 SectorAddr, void *Buffer)
563 ENTER("iDisk XSectorAddr pBuffer", Disk, SectorAddr, Buffer);
565 if( IOCache_Read( gFDD_Devices[Disk].CacheHandle, SectorAddr, Buffer ) == 1 ) {
570 // Pass to general function
571 ret = FDD_int_ReadWriteSector(Disk, SectorAddr, 0, Buffer);
574 IOCache_Add( gFDD_Devices[Disk].CacheHandle, SectorAddr, Buffer );
579 LOG("Reading failed");
586 * \fn int FDD_WriteSector(Uint32 Disk, Uint64 LBA, void *Buffer)
587 * \brief Write a sector to the floppy disk
588 * \note Not Implemented
590 int FDD_WriteSector(Uint32 Disk, Uint64 LBA, void *Buffer)
592 Log_Warning("FDD", "Read Only at the moment");
597 * \brief Seek disk to selected track
599 int FDD_int_SeekTrack(int disk, int head, int track)
602 int base, i, bUnclean;
604 base = cPORTBASE[disk>>1];
606 // Check if seeking is needed
607 if(gFDD_Devices[disk].track[head] == track)
611 for( i = 0; i < FDD_MAX_READWRITE_ATTEMPTS; i ++ )
613 if(i && bUnclean) FDD_Reset(disk >> 1);
615 if(FDD_int_SendByte(base, CMD_SEEK_TRACK)) continue;
616 if(FDD_int_SendByte(base, (head<<2)|(disk&1))) continue;
617 if(FDD_int_SendByte(base, track)) continue;
619 FDD_SenseInt(base, &sr0, &cyl); // Wait for IRQ
623 continue; // Try again
624 if( (sr0 & 0xF0) != 0x20 ) {
625 LOG("sr0 = 0x%x", sr0);
632 if( i == FDD_MAX_READWRITE_ATTEMPTS ) {
633 Log_Warning("FDD", "Unable to seek to track %i on disk %i",
638 // Set Track in structure
639 gFDD_Devices[disk].track[head] = track;
641 LOG("Time_Delay(100)");
642 // Wait for Head to settle
649 * \fn int FDD_int_GetDims(int type, int lba, int *c, int *h, int *s, int *spt)
650 * \brief Get Dimensions of a disk
652 int FDD_int_GetDims(int type, int lba, int *c, int *h, int *s, int *spt)
688 //Log("1440k - lba=%i(0x%x), *s=%i,*c=%i,*h=%i", lba, lba, *s, *c, *h);
706 * \fn void FDD_IRQHandler(int Num)
707 * \brief Handles IRQ6
709 void FDD_IRQHandler(int Num, void *Ptr)
715 * \brief Wait for the FDD IRQ to fire
716 * \return Boolean failure (1 for timeout)
718 inline int FDD_WaitIRQ()
720 tTime end = now() + 2000;
723 while(!gbFDD_IrqFired && now() < end)
726 if( !gbFDD_IrqFired ) {
727 Log_Warning("FDD", "FDD_WaitIRQ - Timeout");
735 void FDD_SenseInt(int base, Uint8 *sr0, Uint8 *cyl)
737 FDD_int_SendByte(base, CMD_SENSE_INTERRUPT);
738 FDD_int_GetByte(base, sr0);
739 FDD_int_GetByte(base, cyl);
743 * void FDD_int_SendByte(int base, char byte)
744 * \brief Sends a command to the controller
746 int FDD_int_SendByte(int base, Uint8 byte)
748 tTime end = now() + 1000; // 1s
750 while( (inb(base + PORT_MAINSTATUS) & 0x80) != 0x80 && now() < end )
751 Threads_Yield(); //Delay
752 // Time_Delay(10); //Delay
754 if( inb(base + PORT_MAINSTATUS) & 0x40 ) {
755 Log_Warning("FDD", "FDD_int_SendByte: DIO set, is this ok?");
761 Log_Warning("FDD", "FDD_int_SendByte: Timeout sending byte 0x%x to base 0x%x", byte, base);
764 outb(base + PORT_DATA, byte);
765 // Log_Debug("FDD", "FDD_int_SendByte: Sent 0x%02x to 0x%x", byte, base);
770 * int FDD_int_GetByte(int base, char byte)
771 * \brief Receive data from fdd controller
773 int FDD_int_GetByte(int base, Uint8 *value)
775 tTime end = now() + 1000; // 1s
777 while( (inb(base + PORT_MAINSTATUS) & 0x80) != 0x80 && now() < end )
780 if( !(inb(base + PORT_MAINSTATUS) & 0x40) ) {
781 Log_Warning("FDD", "FDD_int_GetByte: DIO unset, is this ok?");
787 Log_Warning("FDD", "FDD_int_GetByte: Timeout reading byte from base 0x%x", base);
792 *value = inb(base + PORT_DATA);
794 inb(base + PORT_DATA);
799 * \brief Recalibrate the specified disk
801 void FDD_Recalibrate(int disk)
803 ENTER("idisk", disk);
805 LOG("Starting Motor");
806 FDD_int_StartMotor(disk);
808 while(gFDD_Devices[disk].motorState <= 1) Threads_Yield();
810 LOG("Sending Calibrate Command");
811 FDD_int_SendByte(cPORTBASE[disk>>1], CMD_RECALIBRATE);
812 FDD_int_SendByte(cPORTBASE[disk>>1], disk&1);
814 LOG("Waiting for IRQ");
816 FDD_SenseInt(cPORTBASE[disk>>1], NULL, NULL);
818 LOG("Stopping Motor");
819 FDD_int_StopMotor(disk);
824 * \brief Reconfigure the controller
826 int FDD_Reconfigure(int ID)
828 Uint16 base = cPORTBASE[ID];
832 FDD_int_SendByte(base, CMD_CONFIGURE);
833 FDD_int_SendByte(base, 0);
834 // Implied seek enabled, FIFO Enabled, Drive Polling Disabled, data buffer threshold 8 bytes
835 FDD_int_SendByte(base, (1 << 6) | (0 << 5) | (0 << 4) | 7);
836 FDD_int_SendByte(base, 0); // Precompensation - use default
839 FDD_int_SendByte(base, CMD_LOCK|CMD_FLAG_MULTI_TRACK);
840 FDD_int_GetByte(base, NULL);
847 * \brief Reset the specified FDD controller
849 int FDD_Reset(int id)
851 Uint16 base = cPORTBASE[id];
857 motor_state = inb(base + PORT_DIGOUTPUT) & 0xF0;
858 outb(base + PORT_DIGOUTPUT, motor_state|0); // Disable FDC
860 outb(base + PORT_DIGOUTPUT, motor_state|8|4); // Re-enable FDC (DMA and Enable)
863 outb(base + PORT_DATARATE, 0); // Set data rate to 500K/s
870 FDD_SenseInt(base, NULL, NULL);
873 FDD_int_SendByte(base, CMD_SPECIFY); // Step and Head Load Times
874 FDD_int_SendByte(base, 0xDF); // Step Rate Time, Head Unload Time (Nibble each)
875 FDD_int_SendByte(base, 0x02); // Head Load Time >> 1
877 LOG("Recalibrating Disk");
878 FDD_Recalibrate((id<<1)|0);
879 FDD_Recalibrate((id<<1)|1);
885 * \fn void FDD_int_TimerCallback()
886 * \brief Called by timer
888 void FDD_int_TimerCallback(void *Arg)
890 int disk = (Uint)Arg;
892 if(gFDD_Devices[disk].motorState == 1)
893 gFDD_Devices[disk].motorState = 2;
894 Time_RemoveTimer(gFDD_Devices[disk].timer);
895 gFDD_Devices[disk].timer = -1;
900 * \fn void FDD_int_StartMotor(char disk)
901 * \brief Starts FDD Motor
903 void FDD_int_StartMotor(int disk)
906 if( gFDD_Devices[disk].motorState != 0 ) return ;
908 state = inb( cPORTBASE[ disk>>1 ] + PORT_DIGOUTPUT );
909 state |= 1 << (4+disk);
910 outb( cPORTBASE[ disk>>1 ] + PORT_DIGOUTPUT, state );
911 // Mark as spinning up
912 gFDD_Devices[disk].motorState = 1;
913 // Schedule a timer for when it's up to speed
914 gFDD_Devices[disk].timer = Time_CreateTimer(MOTOR_ON_DELAY, FDD_int_TimerCallback, (void*)(Uint)disk);
918 * \brief Schedule the drive motor to stop
919 * \param Disk Disk number to stop
921 void FDD_int_StopMotor(int Disk)
923 // Ignore if the motor is aready off
924 if( gFDD_Devices[Disk].motorState == 0 ) return ;
926 // Don't double-schedule timer
927 if( gFDD_Devices[Disk].timer != -1 )
929 Time_RemoveTimer( gFDD_Devices[Disk].timer );
932 gFDD_Devices[Disk].timer = Time_CreateTimer(MOTOR_OFF_DELAY, FDD_int_StopMotorCallback, (void*)(Uint)Disk);
936 * \brief Stops FDD Motor
938 void FDD_int_StopMotorCallback(void *Arg)
940 Uint8 state, disk = (Uint)Arg;
942 // Mutex is only locked if disk is in use
943 if( Mutex_IsLocked(&glFDD) ) return ;
945 ENTER("iDisk", disk);
947 // Clear motor on bit
948 state = inb( cPORTBASE[ disk>>1 ] + PORT_DIGOUTPUT );
949 state &= ~( 1 << (4+disk) );
950 outb( cPORTBASE[ disk>>1 ] + PORT_DIGOUTPUT, state );
953 gFDD_Devices[disk].motorState = 0;