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