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