Modules/Storage/FDD/fdd.c

   1 /*\r
   2  * AcessOS 0.1\r
   3  * Floppy Disk Access Code\r
   4  */\r
   5 #define DEBUG   0\r
   6 #include <acess.h>\r
   7 #include <modules.h>\r
   8 #include <fs_devfs.h>\r
   9 #include <tpl_drv_disk.h>\r
  10 #include <dma.h>\r
  11 #include <iocache.h>\r
  12 \r
  13 #define WARN    0\r
  14 \r
  15 // === CONSTANTS ===\r
  16 // --- Current Version\r
  17 #define FDD_VERSION      ((0<<8)|(75))\r
  18 \r
  19 // --- Options\r
  20 #define FDD_SEEK_TIMEOUT        10      // Timeout for a seek operation\r
  21 #define MOTOR_ON_DELAY  500             // Miliseconds\r
  22 #define MOTOR_OFF_DELAY 2000    // Miliseconds\r
  23 \r
  24 // === TYPEDEFS ===\r
  25 /**\r
  26  * \brief Representation of a floppy drive\r
  27  */\r
  28 typedef struct sFloppyDrive\r
  29 {\r
  30          int    type;\r
  31         volatile int    motorState;     //2 - On, 1 - Spinup, 0 - Off\r
  32          int    track[2];\r
  33          int    timer;\r
  34         tVFS_Node       Node;\r
  35         #if !USE_CACHE\r
  36         tIOCache        *CacheHandle;\r
  37         #endif\r
  38 } t_floppyDevice;\r
  39 \r
  40 /**\r
  41  * \brief Cached Sector\r
  42  */\r
  43 typedef struct {\r
  44         Uint64  timestamp;\r
  45         Uint16  disk;\r
  46         Uint16  sector; // Allows 32Mb of addressable space (Plenty for FDD)\r
  47         Uint8   data[512];\r
  48 } t_floppySector;\r
  49 \r
  50 // === CONSTANTS ===\r
  51 static const char       *cFDD_TYPES[] = {"None", "360kB 5.25\"", "1.2MB 5.25\"", "720kB 3.5\"", "1.44MB 3.5\"", "2.88MB 3.5\"" };\r
  52 static const int        cFDD_SIZES[] = { 0, 360*1024, 1200*1024, 720*1024, 1440*1024, 2880*1024 };\r
  53 static const short      cPORTBASE[] = { 0x3F0, 0x370 };\r
  54 \r
  55 enum FloppyPorts {\r
  56         PORT_STATUSA    = 0x0,\r
  57         PORT_STATUSB    = 0x1,\r
  58         PORT_DIGOUTPUT  = 0x2,\r
  59         PORT_MAINSTATUS = 0x4,\r
  60         PORT_DATARATE   = 0x4,\r
  61         PORT_DATA               = 0x5,\r
  62         PORT_DIGINPUT   = 0x7,\r
  63         PORT_CONFIGCTRL = 0x7\r
  64 };\r
  65 \r
  66 enum FloppyCommands {\r
  67         FIX_DRIVE_DATA  = 0x03,\r
  68         HECK_DRIVE_STATUS       = 0x04,\r
  69         CALIBRATE_DRIVE = 0x07,\r
  70         CHECK_INTERRUPT_STATUS = 0x08,\r
  71         SEEK_TRACK              = 0x0F,\r
  72         READ_SECTOR_ID  = 0x4A,\r
  73         FORMAT_TRACK    = 0x4D,\r
  74         READ_TRACK              = 0x42,\r
  75         READ_SECTOR             = 0x66,\r
  76         WRITE_SECTOR    = 0xC5,\r
  77         WRITE_DELETE_SECTOR     = 0xC9,\r
  78         READ_DELETE_SECTOR      = 0xCC,\r
  79 };\r
  80 \r
  81 // === PROTOTYPES ===\r
  82 // --- Filesystem\r
  83  int    FDD_Install(char **Arguments);\r
  84 void    FDD_UnloadModule();\r
  85 // --- VFS Methods\r
  86 char    *FDD_ReadDir(tVFS_Node *Node, int pos);\r
  87 tVFS_Node       *FDD_FindDir(tVFS_Node *dirNode, char *Name);\r
  88  int    FDD_IOCtl(tVFS_Node *Node, int ID, void *Data);\r
  89 Uint64  FDD_ReadFS(tVFS_Node *node, Uint64 off, Uint64 len, void *buffer);\r
  90 // --- Functions for IOCache/DrvUtil\r
  91 Uint    FDD_ReadSectors(Uint64 SectorAddr, Uint Count, void *Buffer, Uint Disk);\r
  92 // --- Raw Disk Access\r
  93  int    FDD_ReadSector(Uint32 disk, Uint64 lba, void *Buffer);\r
  94  int    FDD_WriteSector(Uint32 Disk, Uint64 LBA, void *Buffer);\r
  95 // --- Helpers\r
  96 void    FDD_IRQHandler(int Num);\r
  97 inline void     FDD_WaitIRQ();\r
  98 void    FDD_SensInt(int base, Uint8 *sr0, Uint8 *cyl);\r
  99 void    FDD_int_SendByte(int base, char byte);\r
 100  int    FDD_int_GetByte(int base);\r
 101 void    FDD_Reset(int id);\r
 102 void    FDD_Recalibrate(int disk);\r
 103  int    FDD_int_SeekTrack(int disk, int head, int track);\r
 104 void    FDD_int_TimerCallback(int arg);\r
 105 void    FDD_int_StopMotor(int disk);\r
 106 void    FDD_int_StartMotor(int disk);\r
 107  int    FDD_int_GetDims(int type, int lba, int *c, int *h, int *s, int *spt);\r
 108 \r
 109 // === GLOBALS ===\r
 110 MODULE_DEFINE(0, FDD_VERSION, FDD, FDD_Install, NULL, NULL);\r
 111 t_floppyDevice  gFDD_Devices[2];\r
 112 tSpinlock       glFDD;\r
 113 volatile int    gbFDD_IrqFired = 0;\r
 114 tDevFS_Driver   gFDD_DriverInfo = {\r
 115         NULL, "fdd",\r
 116         {\r
 117         .Size = -1,\r
 118         .NumACLs = 1,\r
 119         .ACLs = &gVFS_ACL_EveryoneRX,\r
 120         .Flags = VFS_FFLAG_DIRECTORY,\r
 121         .ReadDir = FDD_ReadDir,\r
 122         .FindDir = FDD_FindDir,\r
 123         .IOCtl = FDD_IOCtl\r
 124         }\r
 125 };\r
 126 \r
 127 // === CODE ===\r
 128 /**\r
 129  * \fn int FDD_Install(char **Arguments)\r
 130  * \brief Installs floppy driver\r
 131  */\r
 132 int FDD_Install(char **Arguments)\r
 133 {\r
 134         Uint8 data;\r
 135         \r
 136         // Determine Floppy Types (From CMOS)\r
 137         outb(0x70, 0x10);\r
 138         data = inb(0x71);\r
 139         gFDD_Devices[0].type = data >> 4;\r
 140         gFDD_Devices[1].type = data & 0xF;\r
 141         gFDD_Devices[0].track[0] = -1;\r
 142         gFDD_Devices[1].track[1] = -1;\r
 143         \r
 144         Log("[FDD ] Detected Disk 0: %s and Disk 1: %s", cFDD_TYPES[data>>4], cFDD_TYPES[data&0xF]);\r
 145         \r
 146         // Clear FDD IRQ Flag\r
 147         FDD_SensInt(0x3F0, NULL, NULL);\r
 148         // Install IRQ6 Handler\r
 149         IRQ_AddHandler(6, FDD_IRQHandler);\r
 150         // Reset Primary FDD Controller\r
 151         FDD_Reset(0);\r
 152         \r
 153         // Initialise Root Node\r
 154         gFDD_DriverInfo.RootNode.CTime = gFDD_DriverInfo.RootNode.MTime\r
 155                 = gFDD_DriverInfo.RootNode.ATime = now();\r
 156         \r
 157         // Initialise Child Nodes\r
 158         gFDD_Devices[0].Node.Inode = 0;\r
 159         gFDD_Devices[0].Node.Flags = 0;\r
 160         gFDD_Devices[0].Node.NumACLs = 0;\r
 161         gFDD_Devices[0].Node.Read = FDD_ReadFS;\r
 162         gFDD_Devices[0].Node.Write = NULL;//FDD_WriteFS;\r
 163         memcpy(&gFDD_Devices[1].Node, &gFDD_Devices[0].Node, sizeof(tVFS_Node));\r
 164         \r
 165         gFDD_Devices[1].Node.Inode = 1;\r
 166         \r
 167         // Set Lengths\r
 168         gFDD_Devices[0].Node.Size = cFDD_SIZES[data >> 4];\r
 169         gFDD_Devices[1].Node.Size = cFDD_SIZES[data & 0xF];\r
 170         \r
 171         // Create Sector Cache\r
 172         if( cFDD_SIZES[data >> 4] )\r
 173         {\r
 174                 gFDD_Devices[0].CacheHandle = IOCache_Create(\r
 175                         FDD_WriteSector, 0, 512,\r
 176                         gFDD_Devices[0].Node.Size / (512*4)\r
 177                         );      // Cache is 1/4 the size of the disk\r
 178         }\r
 179         if( cFDD_SIZES[data & 15] )\r
 180         {\r
 181                 gFDD_Devices[1].CacheHandle = IOCache_Create(\r
 182                         FDD_WriteSector, 0, 512,\r
 183                         gFDD_Devices[1].Node.Size / (512*4)\r
 184                         );      // Cache is 1/4 the size of the disk\r
 185         }\r
 186         \r
 187         // Register with devfs\r
 188         DevFS_AddDevice(&gFDD_DriverInfo);\r
 189         \r
 190         return MODULE_ERR_OK;\r
 191 }\r
 192 \r
 193 /**\r
 194  * \brief Prepare the module for removal\r
 195  */\r
 196 void FDD_UnloadModule()\r
 197 {\r
 198          int    i;\r
 199         //DevFS_DelDevice( &gFDD_DriverInfo );\r
 200         LOCK(&glFDD);\r
 201         for(i=0;i<4;i++) {\r
 202                 Time_RemoveTimer(gFDD_Devices[i].timer);\r
 203                 FDD_int_StopMotor(i);\r
 204         }\r
 205         RELEASE(&glFDD);\r
 206         //IRQ_Clear(6);\r
 207 }\r
 208 \r
 209 /**\r
 210  * \fn char *FDD_ReadDir(tVFS_Node *Node, int pos)\r
 211  * \brief Read Directory\r
 212  */\r
 213 char *FDD_ReadDir(tVFS_Node *Node, int Pos)\r
 214 {\r
 215         char    name[2] = "0\0";\r
 216 \r
 217         if(Pos >= 2 || Pos < 0) return NULL;\r
 218         \r
 219         if(gFDD_Devices[Pos].type == 0) return VFS_SKIP;\r
 220         \r
 221         name[0] += Pos;\r
 222         \r
 223         return strdup(name);\r
 224 }\r
 225 \r
 226 /**\r
 227  * \fn tVFS_Node *FDD_FindDir(tVFS_Node *Node, char *filename);\r
 228  * \brief Find File Routine (for vfs_node)\r
 229  */\r
 230 tVFS_Node *FDD_FindDir(tVFS_Node *Node, char *Filename)\r
 231 {\r
 232          int    i;\r
 233         \r
 234         ENTER("sFilename", Filename);\r
 235         \r
 236         // Sanity check string\r
 237         if(Filename == NULL) {\r
 238                 LEAVE('n');\r
 239                 return NULL;\r
 240         }\r
 241         \r
 242         // Check string length (should be 1)\r
 243         if(Filename[0] == '\0' || Filename[1] != '\0') {\r
 244                 LEAVE('n');\r
 245                 return NULL;\r
 246         }\r
 247         \r
 248         // Get First character\r
 249         i = Filename[0] - '0';\r
 250         \r
 251         // Check for 1st disk and if it is present return\r
 252         if(i == 0 && gFDD_Devices[0].type != 0) {\r
 253                 LEAVE('p', &gFDD_Devices[0].Node);\r
 254                 return &gFDD_Devices[0].Node;\r
 255         }\r
 256         \r
 257         // Check for 2nd disk and if it is present return\r
 258         if(i == 1 && gFDD_Devices[1].type != 0) {\r
 259                 LEAVE('p', &gFDD_Devices[1].Node);\r
 260                 return &gFDD_Devices[1].Node;\r
 261         }\r
 262         \r
 263         // Else return null\r
 264         LEAVE('n');\r
 265         return NULL;\r
 266 }\r
 267 \r
 268 static const char       *casIOCTLS[] = {DRV_IOCTLNAMES,DRV_DISK_IOCTLNAMES,NULL};\r
 269 /**\r
 270  * \fn int FDD_IOCtl(tVFS_Node *Node, int id, void *data)\r
 271  * \brief Stub ioctl function\r
 272  */\r
 273 int FDD_IOCtl(tVFS_Node *Node, int ID, void *Data)\r
 274 {\r
 275         switch(ID)\r
 276         {\r
 277         case DRV_IOCTL_TYPE:    return DRV_TYPE_DISK;\r
 278         case DRV_IOCTL_IDENT:   return ModUtil_SetIdent(Data, "FDD");\r
 279         case DRV_IOCTL_VERSION: return FDD_VERSION;\r
 280         case DRV_IOCTL_LOOKUP:  return ModUtil_LookupString((char**)casIOCTLS, Data);\r
 281         \r
 282         case DISK_IOCTL_GETBLOCKSIZE:   return 512;     \r
 283         \r
 284         default:\r
 285                 return 0;\r
 286         }\r
 287 }\r
 288 \r
 289 /**\r
 290  * \fn Uint64 FDD_ReadFS(tVFS_Node *Node, Uint64 Offset, Uint64 Length, void *Buffer)\r
 291  * \brief Read Data from a disk\r
 292 */\r
 293 Uint64 FDD_ReadFS(tVFS_Node *Node, Uint64 Offset, Uint64 Length, void *Buffer)\r
 294 {\r
 295          int    ret;\r
 296         \r
 297         ENTER("pNode XOffset XLength pBuffer", Node, Offset, Length, Buffer);\r
 298         \r
 299         if(Node == NULL) {\r
 300                 LEAVE('i', -1);\r
 301                 return -1;\r
 302         }\r
 303         \r
 304         if(Node->Inode != 0 && Node->Inode != 1) {\r
 305                 LEAVE('i', -1);\r
 306                 return -1;\r
 307         }\r
 308         \r
 309         ret = DrvUtil_ReadBlock(Offset, Length, Buffer, FDD_ReadSectors, 512, Node->Inode);\r
 310         LEAVE('i', ret);\r
 311         return ret;\r
 312 }\r
 313 \r
 314 /**\r
 315  * \brief Reads \a Count contiguous sectors from a disk\r
 316  * \param SectorAddr    Address of the first sector\r
 317  * \param Count Number of sectors to read\r
 318  * \param Buffer        Destination Buffer\r
 319  * \param Disk  Disk Number\r
 320  * \return Number of sectors read\r
 321  * \note Used as a ::DrvUtil_ReadBlock helper\r
 322  */\r
 323 Uint FDD_ReadSectors(Uint64 SectorAddr, Uint Count, void *Buffer, Uint Disk)\r
 324 {\r
 325         Uint    ret = 0;\r
 326         while(Count --)\r
 327         {\r
 328                 if( FDD_ReadSector(Disk, SectorAddr, Buffer) != 1 )\r
 329                         return ret;\r
 330                 \r
 331                 Buffer = (void*)( (tVAddr)Buffer + 512 );\r
 332                 SectorAddr ++;\r
 333                 ret ++;\r
 334         }\r
 335         return ret;\r
 336 }\r
 337 \r
 338 /**\r
 339  * \fn int FDD_ReadSector(Uint32 Disk, Uint64 SectorAddr, void *Buffer)\r
 340  * \brief Read a sector from disk\r
 341  * \todo Make real-hardware safe (account for read errors)\r
 342 */\r
 343 int FDD_ReadSector(Uint32 Disk, Uint64 SectorAddr, void *Buffer)\r
 344 {\r
 345          int    cyl, head, sec;\r
 346          int    spt, base;\r
 347          int    i;\r
 348          int    lba = SectorAddr;\r
 349         \r
 350         ENTER("iDisk XSectorAddr pBuffer", disk, SectorAddr, Buffer);\r
 351         \r
 352         #if USE_CACHE\r
 353         FDD_AquireCacheSpinlock();\r
 354         for( i = 0; i < siFDD_SectorCacheSize; i++ )\r
 355         {\r
 356                 if(sFDD_SectorCache[i].timestamp == 0)  continue;\r
 357                 if( sFDD_SectorCache[i].disk == Disk\r
 358                  && sFDD_SectorCache[i].sector == lba)\r
 359                 {\r
 360                         LOG("Found %i in cache %i", lba, i);\r
 361                         memcpy(Buffer, sFDD_SectorCache[i].data, 512);\r
 362                         sFDD_SectorCache[i].timestamp = now();\r
 363                         FDD_FreeCacheSpinlock();\r
 364                         LEAVE('i', 1);\r
 365                         return 1;\r
 366                 }\r
 367         }\r
 368         LOG("Read %i from Disk", lba);\r
 369         FDD_FreeCacheSpinlock();\r
 370         #else\r
 371         if( IOCache_Read( gFDD_Devices[Disk].CacheHandle, SectorAddr, Buffer ) == 1 ) {\r
 372                 LEAVE('i', 1);\r
 373                 return 1;\r
 374         }\r
 375         #endif\r
 376         \r
 377         base = cPORTBASE[Disk >> 1];\r
 378         \r
 379         LOG("Calculating Disk Dimensions");\r
 380         // Get CHS position\r
 381         if(FDD_int_GetDims(gFDD_Devices[Disk].type, lba, &cyl, &head, &sec, &spt) != 1)\r
 382         {\r
 383                 LEAVE('i', -1);\r
 384                 return -1;\r
 385         }\r
 386         \r
 387         // Remove Old Timer\r
 388         Time_RemoveTimer(gFDD_Devices[Disk].timer);\r
 389         // Check if Motor is on\r
 390         if(gFDD_Devices[Disk].motorState == 0)  FDD_int_StartMotor(Disk);\r
 391         \r
 392         LOG("Wait for the motor to spin up");\r
 393         \r
 394         // Wait for spinup\r
 395         while(gFDD_Devices[Disk].motorState == 1)       Threads_Yield();\r
 396         \r
 397         LOG("Cyl=%i, Head=%i, Sector=%i", cyl, head, sec);\r
 398         LOG("Acquire Spinlock");\r
 399         \r
 400         LOCK(&glFDD);\r
 401         \r
 402         // Seek to track\r
 403         outb(base + CALIBRATE_DRIVE, 0);\r
 404         i = 0;\r
 405         while(FDD_int_SeekTrack(Disk, head, (Uint8)cyl) == 0 && i++ < FDD_SEEK_TIMEOUT )\r
 406                 Threads_Yield();\r
 407         if( i > FDD_SEEK_TIMEOUT ) {\r
 408                 LEAVE('i', 0);\r
 409                 return 0;\r
 410         }\r
 411         //FDD_SensInt(base, NULL, NULL);        // Wait for IRQ\r
 412                 \r
 413         // Read Data from DMA\r
 414         LOG("Setting DMA for read");\r
 415         DMA_SetChannel(2, 512, 1);      // Read 512 Bytes from channel 2\r
 416         \r
 417         LOG("Sending read command");\r
 418         \r
 419         //Threads_Wait(100);    // Wait for Head to settle\r
 420         Time_Delay(100);\r
 421         FDD_int_SendByte(base, READ_SECTOR);    // Was 0xE6\r
 422         FDD_int_SendByte(base, (head << 2) | (Disk&1));\r
 423         FDD_int_SendByte(base, (Uint8)cyl);\r
 424         FDD_int_SendByte(base, (Uint8)head);\r
 425         FDD_int_SendByte(base, (Uint8)sec);\r
 426         FDD_int_SendByte(base, 0x02);   // Bytes Per Sector (Real BPS=128*2^{val})\r
 427         FDD_int_SendByte(base, spt);    // SPT\r
 428         FDD_int_SendByte(base, 0x1B);   // Gap Length (27 is default)\r
 429         FDD_int_SendByte(base, 0xFF);   // Data Length\r
 430         \r
 431         // Wait for IRQ\r
 432         LOG("Waiting for Data to be read");\r
 433         FDD_WaitIRQ();\r
 434         \r
 435         // Read Data from DMA\r
 436         LOG(" FDD_ReadSector: Reading Data");\r
 437         DMA_ReadData(2, 512, Buffer);\r
 438         \r
 439         // Clear Input Buffer\r
 440         LOG("Clearing Input Buffer");\r
 441         FDD_int_GetByte(base); FDD_int_GetByte(base); FDD_int_GetByte(base);\r
 442         FDD_int_GetByte(base); FDD_int_GetByte(base); FDD_int_GetByte(base); FDD_int_GetByte(base);\r
 443         \r
 444         // Release Spinlock\r
 445         LOG("Realeasing Spinlock and setting motor to stop");\r
 446         RELEASE(&glFDD);\r
 447         \r
 448         // Don't turn the motor off now, wait for a while\r
 449         gFDD_Devices[Disk].timer = Time_CreateTimer(MOTOR_OFF_DELAY, FDD_int_StopMotor, (void*)Disk);
 450 \r
 451         IOCache_Add( gFDD_Devices[Disk].CacheHandle, SectorAddr, Buffer );\r
 452
 453         LEAVE('i', 1);\r
 454         return 1;\r
 455 }\r
 456 \r
 457 /**\r
 458  * \fn int FDD_WriteSector(Uint32 Disk, Uint64 LBA, void *Buffer)\r
 459  * \brief Write a sector to the floppy disk\r
 460  * \note Not Implemented\r
 461  */\r
 462 int FDD_WriteSector(Uint32 Disk, Uint64 LBA, void *Buffer)\r
 463 {\r
 464         Warning("[FDD  ] Read Only at the moment");\r
 465         return -1;\r
 466 }\r
 467 \r
 468 /**\r
 469  * \fn int FDD_int_SeekTrack(int disk, int track)\r
 470  * \brief Seek disk to selected track\r
 471  */\r
 472 int FDD_int_SeekTrack(int disk, int head, int track)\r
 473 {\r
 474         Uint8   sr0, cyl;\r
 475          int    base;\r
 476         \r
 477         base = cPORTBASE[disk>>1];\r
 478         \r
 479         // Check if seeking is needed\r
 480         if(gFDD_Devices[disk].track[head] == track)\r
 481                 return 1;\r
 482         \r
 483         // - Seek Head 0\r
 484         FDD_int_SendByte(base, SEEK_TRACK);\r
 485         FDD_int_SendByte(base, (head<<2)|(disk&1));\r
 486         FDD_int_SendByte(base, track);  // Send Seek command\r
 487         FDD_WaitIRQ();\r
 488         FDD_SensInt(base, &sr0, &cyl);  // Wait for IRQ\r
 489         if((sr0 & 0xF0) != 0x20) {
 490                 LOG("sr0 = 0x%x", sr0);
 491                 return 0;       //Check Status
 492         }\r
 493         if(cyl != track)        return 0;\r
 494         \r
 495         // Set Track in structure\r
 496         gFDD_Devices[disk].track[head] = track;\r
 497         return 1;\r
 498 }\r
 499 \r
 500 /**\r
 501  * \fn int FDD_int_GetDims(int type, int lba, int *c, int *h, int *s, int *spt)\r
 502  * \brief Get Dimensions of a disk\r
 503  */\r
 504 int FDD_int_GetDims(int type, int lba, int *c, int *h, int *s, int *spt)\r
 505 {\r
 506         switch(type) {\r
 507         case 0:\r
 508                 return 0;\r
 509         \r
 510         // 360Kb 5.25"\r
 511         case 1:\r
 512                 *spt = 9;\r
 513                 *s = (lba % 9) + 1;\r
 514                 *c = lba / 18;\r
 515                 *h = (lba / 9) & 1;\r
 516                 break;\r
 517         \r
 518         // 1220Kb 5.25"\r
 519         case 2:\r
 520                 *spt = 15;\r
 521                 *s = (lba % 15) + 1;\r
 522                 *c = lba / 30;\r
 523                 *h = (lba / 15) & 1;\r
 524                 break;\r
 525         \r
 526         // 720Kb 3.5"\r
 527         case 3:\r
 528                 *spt = 9;\r
 529                 *s = (lba % 9) + 1;\r
 530                 *c = lba / 18;\r
 531                 *h = (lba / 9) & 1;\r
 532                 break;\r
 533         \r
 534         // 1440Kb 3.5"\r
 535         case 4:\r
 536                 *spt = 18;\r
 537                 *s = (lba % 18) + 1;\r
 538                 *c = lba / 36;\r
 539                 *h = (lba / 18) & 1;\r
 540                 //Log("1440k - lba=%i(0x%x), *s=%i,*c=%i,*h=%i", lba, lba, *s, *c, *h);\r
 541                 break;\r
 542                 \r
 543         // 2880Kb 3.5"\r
 544         case 5:\r
 545                 *spt = 36;\r
 546                 *s = (lba % 36) + 1;\r
 547                 *c = lba / 72;\r
 548                 *h = (lba / 32) & 1;\r
 549                 break;\r
 550                 \r
 551         default:\r
 552                 return -2;\r
 553         }\r
 554         return 1;\r
 555 }\r
 556 \r
 557 /**\r
 558  * \fn void FDD_IRQHandler(int Num)\r
 559  * \brief Handles IRQ6\r
 560  */\r
 561 void FDD_IRQHandler(int Num)\r
 562 {\r
 563     gbFDD_IrqFired = 1;\r
 564 }\r
 565 \r
 566 /**\r
 567  * \fn FDD_WaitIRQ()\r
 568  * \brief Wait for an IRQ6\r
 569  */\r
 570 inline void FDD_WaitIRQ()\r
 571 {\r
 572         // Wait for IRQ\r
 573         while(!gbFDD_IrqFired)  Threads_Yield();\r
 574         gbFDD_IrqFired = 0;\r
 575 }\r
 576 \r
 577 void FDD_SensInt(int base, Uint8 *sr0, Uint8 *cyl)\r
 578 {\r
 579         FDD_int_SendByte(base, CHECK_INTERRUPT_STATUS);\r
 580         if(sr0) *sr0 = FDD_int_GetByte(base);\r
 581         else    FDD_int_GetByte(base);\r
 582         if(cyl) *cyl = FDD_int_GetByte(base);\r
 583         else    FDD_int_GetByte(base);\r
 584 }\r
 585 \r
 586 /**\r
 587  * void FDD_int_SendByte(int base, char byte)\r
 588  * \brief Sends a command to the controller\r
 589  */\r
 590 void FDD_int_SendByte(int base, char byte)\r
 591 {\r
 592     volatile int state;\r
 593     int timeout = 128;\r
 594     for( ; timeout--; )\r
 595     {\r
 596         state = inb(base + PORT_MAINSTATUS);\r
 597         if ((state & 0xC0) == 0x80)\r
 598         {\r
 599             outb(base + PORT_DATA, byte);\r
 600             return;\r
 601         }\r
 602         inb(0x80);      //Delay\r
 603     }\r
 604         \r
 605         #if WARN\r
 606         Warning("FDD_int_SendByte - Timeout sending byte 0x%x to base 0x%x\n", byte, base);\r
 607         #endif\r
 608 }\r
 609 \r
 610 /**\r
 611  * int FDD_int_GetByte(int base, char byte)\r
 612  * \brief Receive data from fdd controller\r
 613  */\r
 614 int FDD_int_GetByte(int base)\r
 615 {\r
 616     volatile int state;\r
 617     int timeout;\r
 618     for( timeout = 128; timeout--; )\r
 619     {\r
 620         state = inb((base + PORT_MAINSTATUS));\r
 621         if ((state & 0xd0) == 0xd0)\r
 622                 return inb(base + PORT_DATA);\r
 623         inb(0x80);\r
 624     }\r
 625     return -1;\r
 626 }\r
 627 \r
 628 /**\r
 629  * \brief Recalibrate the specified disk\r
 630  */\r
 631 void FDD_Recalibrate(int disk)\r
 632 {\r
 633         ENTER("idisk", disk);\r
 634         \r
 635         LOG("Starting Motor");\r
 636         FDD_int_StartMotor(disk);\r
 637         // Wait for Spinup\r
 638         while(gFDD_Devices[disk].motorState == 1)       Threads_Yield();\r
 639         \r
 640         LOG("Sending Calibrate Command");\r
 641         FDD_int_SendByte(cPORTBASE[disk>>1], CALIBRATE_DRIVE);\r
 642         FDD_int_SendByte(cPORTBASE[disk>>1], disk&1);\r
 643         \r
 644         LOG("Waiting for IRQ");\r
 645         FDD_WaitIRQ();\r
 646         FDD_SensInt(cPORTBASE[disk>>1], NULL, NULL);\r
 647         \r
 648         LOG("Stopping Motor");\r
 649         FDD_int_StopMotor(disk);\r
 650         LEAVE('-');\r
 651 }\r
 652 \r
 653 /**\r
 654  * \brief Reset the specified FDD controller\r
 655  */\r
 656 void FDD_Reset(int id)\r
 657 {\r
 658         int base = cPORTBASE[id];\r
 659         \r
 660         ENTER("iID", id);\r
 661         \r
 662         outb(base + PORT_DIGOUTPUT, 0); // Stop Motors & Disable FDC\r
 663         outb(base + PORT_DIGOUTPUT, 0x0C);      // Re-enable FDC (DMA and Enable)\r
 664         \r
 665         LOG("Awaiting IRQ");\r
 666         \r
 667         FDD_WaitIRQ();\r
 668         FDD_SensInt(base, NULL, NULL);\r
 669         \r
 670         LOG("Setting Driver Info");\r
 671         outb(base + PORT_DATARATE, 0);  // Set data rate to 500K/s\r
 672         FDD_int_SendByte(base, FIX_DRIVE_DATA); // Step and Head Load Times\r
 673         FDD_int_SendByte(base, 0xDF);   // Step Rate Time, Head Unload Time (Nibble each)\r
 674         FDD_int_SendByte(base, 0x02);   // Head Load Time >> 1\r
 675         while(FDD_int_SeekTrack(0, 0, 1) == 0); // set track\r
 676         while(FDD_int_SeekTrack(0, 1, 1) == 0); // set track\r
 677         \r
 678         LOG("Recalibrating Disk");\r
 679         FDD_Recalibrate((id<<1)|0);\r
 680         FDD_Recalibrate((id<<1)|1);
 681 \r
 682         LEAVE('-');\r
 683 }\r
 684 \r
 685 /**\r
 686  * \fn void FDD_int_TimerCallback()\r
 687  * \brief Called by timer\r
 688  */\r
 689 void FDD_int_TimerCallback(int arg)\r
 690 {\r
 691         ENTER("iarg", arg);\r
 692         if(gFDD_Devices[arg].motorState == 1)\r
 693                 gFDD_Devices[arg].motorState = 2;\r
 694         Time_RemoveTimer(gFDD_Devices[arg].timer);\r
 695         gFDD_Devices[arg].timer = -1;\r
 696         LEAVE('-');\r
 697 }\r
 698 \r
 699 /**\r
 700  * \fn void FDD_int_StartMotor(char disk)\r
 701  * \brief Starts FDD Motor\r
 702  */\r
 703 void FDD_int_StartMotor(int disk)\r
 704 {\r
 705         Uint8   state;\r
 706         state = inb( cPORTBASE[ disk>>1 ] + PORT_DIGOUTPUT );\r
 707         state |= 1 << (4+disk);\r
 708         outb( cPORTBASE[ disk>>1 ] + PORT_DIGOUTPUT, state );\r
 709         gFDD_Devices[disk].motorState = 1;\r
 710         gFDD_Devices[disk].timer = Time_CreateTimer(MOTOR_ON_DELAY, FDD_int_TimerCallback, (void*)disk);\r
 711 }\r
 712 \r
 713 /**\r
 714  * \fn void FDD_int_StopMotor(int disk)\r
 715  * \brief Stops FDD Motor\r
 716  */\r
 717 void FDD_int_StopMotor(int disk)\r
 718 {\r
 719         Uint8   state;\r
 720         state = inb( cPORTBASE[ disk>>1 ] + PORT_DIGOUTPUT );\r
 721         state &= ~( 1 << (4+disk) );\r
 722         outb( cPORTBASE[ disk>>1 ] + PORT_DIGOUTPUT, state );\r
 723     gFDD_Devices[disk].motorState = 0;\r
 724 }\r