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