Modules/FDD - Trying to fix fdd behavior

[tpg/acess2.git] / Modules / Storage / FDD / fdd.c

diff --git a/Modules/Storage/FDD/fdd.c b/Modules/Storage/FDD/fdd.c
index f83a006..e507e94 100644 (file)
--- 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>
 
@@ -107,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);
  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);
@@ -171,7 +172,22 @@ int FDD_Install(char **Arguments)
        }
        
        // 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
        if( FDD_Reset(0) != 0 ) {
                return MODULE_ERR_MISC;
@@ -568,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];
@@ -591,7 +607,8 @@ 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);
        
@@ -659,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;
 }
@@ -690,8 +707,13 @@ int FDD_int_SendByte(int base, Uint8 byte)
 {
        tTime   end = now() + 1000;     // 1s
        
-       while( (inb(base + PORT_MAINSTATUS) & 0xC0) != 0x80 && now() < end )
+       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?");
+               return -2;
+       }
        
        if( now() < end )
        {
@@ -714,9 +736,14 @@ int FDD_int_GetByte(int base, Uint8 *value)
 {
        tTime   end = now() + 1000;     // 1s
        
-       while( (inb(base + PORT_MAINSTATUS) & 0xd0) != 0xd0 && now() < end )
+       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?");
+               return -2;
+       }
+
        if( now() < end )
        {
                Uint8   tmp = inb(base + PORT_DATA);
@@ -793,38 +820,44 @@ int FDD_Reset(int id)
        outb(base + PORT_DIGOUTPUT, 0); // Disable FDC
        // Wait 4 microseconds - or use 1 thread delay
        Threads_Yield();
-       outb(base + PORT_DIGOUTPUT, 0x0C);      // Re-enable FDC (DMA and Enable)
+       Threads_Yield();
+       outb(base + PORT_DIGOUTPUT, 8|4);       // 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);
        
-       // Set the data rate
-       outb(base + PORT_DATARATE, 0);  // Set data rate to 500K/s
-
-       // Configure
+       // 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 --);   // set track
-       if(retries < 0) return -1;
+       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); // set track
-       if(retries < 0) return -1;
+       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);
 
-       LEAVE('i',0);
-       return 0;
+       LEAVE_RET('i', 0);
 }
 
 /**