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