Modules/FDD/fdd.c

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