Kernel/drv/fdd.c

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