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 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", 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:    return DRV_TYPE_DISK;\r
 281         case DRV_IOCTL_IDENT:   return ModUtil_SetIdent(Data, "FDD");\r
 282         case DRV_IOCTL_VERSION: return FDD_VERSION;\r
 283         case DRV_IOCTL_LOOKUP:  return ModUtil_LookupString((char**)casIOCTLS, Data);\r
 284         \r
 285         case DISK_IOCTL_GETBLOCKSIZE:   return 512;     \r
 286         \r
 287         default:\r
 288                 return 0;\r
 289         }\r
 290 }\r
 291 \r
 292 /**\r
 293  * \fn Uint64 FDD_ReadFS(tVFS_Node *Node, Uint64 Offset, Uint64 Length, void *Buffer)\r
 294  * \brief Read Data from a disk\r
 295 */\r
 296 Uint64 FDD_ReadFS(tVFS_Node *Node, Uint64 Offset, Uint64 Length, void *Buffer)\r
 297 {\r
 298          int    i = 0;\r
 299          int    disk;\r
 300         //Uint32        buf[128];\r
 301         \r
 302         ENTER("pNode XOffset XLength pBuffer", Node, Offset, Length, Buffer);\r
 303         \r
 304         if(Node == NULL) {\r
 305                 LEAVE('i', -1);\r
 306                 return -1;\r
 307         }\r
 308         \r
 309         if(Node->Inode != 0 && Node->Inode != 1) {\r
 310                 LEAVE('i', -1);\r
 311                 return -1;\r
 312         }\r
 313         \r
 314         disk = Node->Inode;\r
 315         \r
 316         // Update Accessed Time\r
 317         Node->ATime = now();\r
 318         \r
 319         #if 0\r
 320         if((Offset & 0x1FF) || (Length & 0x1FF))\r
 321         {\r
 322                 // Un-Aligned Offset/Length\r
 323                  int    startOff = Offset >> 9;\r
 324                  int    sectOff = Offset & 0x1FF;\r
 325                  int    sectors = (Length + 0x1FF) >> 9;\r
 326         \r
 327                 LOG("Non-aligned Read");\r
 328                 \r
 329                 //Read Starting Sector\r
 330                 if(!FDD_ReadSector(disk, startOff, buf))\r
 331                         return 0;\r
 332                 memcpy(Buffer, (char*)(buf+sectOff), Length > 512-sectOff ? 512-sectOff : Length);\r
 333                 \r
 334                 // If the data size is one sector or less\r
 335                 if(Length <= 512-sectOff)\r
 336                 {\r
 337                         LEAVE('X', Length);\r
 338                         return Length;  //Return\r
 339                 }\r
 340                 Buffer += 512-sectOff;\r
 341         \r
 342                 //Read Middle Sectors\r
 343                 for( i = 1; i < sectors - 1; i++ )\r
 344                 {\r
 345                         if(!FDD_ReadSector(disk, startOff+i, buf)) {\r
 346                                 LEAVE('i', -1);\r
 347                                 return -1;\r
 348                         }\r
 349                         memcpy(Buffer, buf, 512);\r
 350                         Buffer += 512;\r
 351                 }\r
 352         \r
 353                 //Read End Sectors\r
 354                 if(!FDD_ReadSector(disk, startOff+i, buf))\r
 355                         return 0;\r
 356                 memcpy(Buffer, buf, (len&0x1FF)-sectOff);\r
 357
 358                 LEAVE('X', Length);\r
 359                 return Length;\r
 360         }\r
 361         else\r
 362         {\r
 363                  int    count = Length >> 9;\r
 364                  int    sector = Offset >> 9;\r
 365                 LOG("Aligned Read");\r
 366                 //Aligned Offset and Length - Simple Code\r
 367                 for( i = 0; i < count; i ++ )\r
 368                 {\r
 369                         FDD_ReadSector(disk, sector, buf);\r
 370                         memcpy(buffer, buf, 512);\r
 371                         buffer += 512;\r
 372                         sector++;\r
 373                 }
 374                 LEAVE('i', Length);\r
 375                 return Length;\r
 376         }\r
 377         #endif\r
 378         \r
 379         i = DrvUtil_ReadBlock(Offset, Length, Buffer, FDD_ReadSectors, 512, disk);\r
 380         LEAVE('i', i);\r
 381         return i;\r
 382 }\r
 383 \r
 384 /**\r
 385  * \fn Uint FDD_ReadSectors(Uint64 SectorAddr, Uint Count, void *Buffer, Uint32 Disk)\r
 386  * \brief Reads \a Count contiguous sectors from a disk\r
 387  * \param SectorAddr    Address of the first sector\r
 388  * \param Count Number of sectors to read\r
 389  * \param Buffer        Destination Buffer\r
 390  * \param Disk  Disk Number\r
 391  * \return Number of sectors read\r
 392  * \note Used as a ::DrvUtil_ReadBlock helper\r
 393  */\r
 394 Uint FDD_ReadSectors(Uint64 SectorAddr, Uint Count, void *Buffer, Uint Disk)\r
 395 {\r
 396         Uint    ret = 0;\r
 397         while(Count --)\r
 398         {\r
 399                 if( FDD_ReadSector(Disk, SectorAddr, Buffer) != 1 )\r
 400                         return ret;\r
 401                 \r
 402                 Buffer = (void*)( (tVAddr)Buffer + 512 );\r
 403                 SectorAddr ++;\r
 404                 ret ++;\r
 405         }\r
 406         return ret;\r
 407 }\r
 408 \r
 409 /**\r
 410  * \fn int FDD_ReadSector(Uint32 Disk, Uint64 SectorAddr, void *Buffer)\r
 411  * \brief Read a sector from disk\r
 412  * \todo Make real-hardware safe (account for read errors)\r
 413 */\r
 414 int FDD_ReadSector(Uint32 Disk, Uint64 SectorAddr, void *Buffer)\r
 415 {\r
 416          int    cyl, head, sec;\r
 417          int    spt, base;\r
 418          int    i;\r
 419          int    lba = SectorAddr;\r
 420         \r
 421         ENTER("idisk Xlba pbuf", disk, lba, buf);\r
 422         \r
 423         #if USE_CACHE\r
 424         FDD_AquireCacheSpinlock();\r
 425         for( i = 0; i < siFDD_SectorCacheSize; i++ )\r
 426         {\r
 427                 if(sFDD_SectorCache[i].timestamp == 0)  continue;\r
 428                 if(sFDD_SectorCache[i].disk == Disk\r
 429                 && sFDD_SectorCache[i].sector == lba) {\r
 430                         LOG("Found %i in cache %i", lba, i);\r
 431                         memcpy(Buffer, sFDD_SectorCache[i].data, 512);\r
 432                         sFDD_SectorCache[i].timestamp = now();\r
 433                         FDD_FreeCacheSpinlock();\r
 434                         LEAVE('i', 1);\r
 435                         return 1;\r
 436                 }\r
 437         }\r
 438         LOG("Read %i from Disk", lba);\r
 439         FDD_FreeCacheSpinlock();\r
 440         #else\r
 441         if( IOCache_Read( gFDD_Devices[Disk].CacheHandle, SectorAddr, Buffer ) == 1 ) {\r
 442                 LEAVE('i', 1);\r
 443                 return 1;\r
 444         }\r
 445         #endif\r
 446         \r
 447         base = cPORTBASE[Disk>>1];\r
 448         \r
 449         LOG("Calculating Disk Dimensions");\r
 450         // Get CHS position\r
 451         if(FDD_int_GetDims(gFDD_Devices[Disk].type, lba, &cyl, &head, &sec, &spt) != 1) {\r
 452                 LEAVE('i', -1);\r
 453                 return -1;\r
 454         }\r
 455         \r
 456         // Remove Old Timer\r
 457         Time_RemoveTimer(gFDD_Devices[Disk].timer);\r
 458         // Check if Motor is on\r
 459         if(gFDD_Devices[Disk].motorState == 0) {\r
 460                 FDD_int_StartMotor(Disk);\r
 461         }\r
 462         \r
 463         LOG("Wait for Motor Spinup");\r
 464         \r
 465         // Wait for spinup\r
 466         while(gFDD_Devices[Disk].motorState == 1)       Threads_Yield();\r
 467         \r
 468         LOG("C:%i,H:%i,S:%i", cyl, head, sec);\r
 469         LOG("Acquire Spinlock");\r
 470         \r
 471         FDD_AquireSpinlock();\r
 472         \r
 473         // Seek to track\r
 474         outb(base+CALIBRATE_DRIVE, 0);\r
 475         i = 0;\r
 476         while(FDD_int_SeekTrack(Disk, head, (Uint8)cyl) == 0 && i++ < FDD_SEEK_TIMEOUT )        Threads_Yield();\r
 477         //FDD_SensInt(base, NULL, NULL);        // Wait for IRQ\r
 478         \r
 479         LOG("Setting DMA for read");\r
 480         \r
 481         //Read Data from DMA\r
 482         DMA_SetChannel(2, 512, 1);      // Read 512 Bytes\r
 483         \r
 484         LOG("Sending read command");\r
 485         \r
 486         //Threads_Wait(100);    // Wait for Head to settle\r
 487         Time_Delay(100);\r
 488         FDD_int_SendByte(base, READ_SECTOR);    // Was 0xE6\r
 489         FDD_int_SendByte(base, (head << 2) | (Disk&1));\r
 490         FDD_int_SendByte(base, (Uint8)cyl);\r
 491         FDD_int_SendByte(base, (Uint8)head);\r
 492         FDD_int_SendByte(base, (Uint8)sec);\r
 493         FDD_int_SendByte(base, 0x02);   // Bytes Per Sector (Real BPS=128*2^{val})\r
 494         FDD_int_SendByte(base, spt);    // SPT\r
 495         FDD_int_SendByte(base, 0x1B);   // Gap Length (27 is default)\r
 496         FDD_int_SendByte(base, 0xFF);   // Data Length\r
 497         \r
 498         // Wait for IRQ\r
 499         LOG("Waiting for Data to be read");\r
 500         FDD_WaitIRQ();\r
 501         \r
 502         // Read Data from DMA\r
 503         LOG(" FDD_ReadSector: Reading Data");\r
 504         DMA_ReadData(2, 512, Buffer);\r
 505         \r
 506         // Clear Input Buffer\r
 507         LOG("Clearing Input Buffer");\r
 508         FDD_int_GetByte(base); FDD_int_GetByte(base); FDD_int_GetByte(base);\r
 509         FDD_int_GetByte(base); FDD_int_GetByte(base); FDD_int_GetByte(base); FDD_int_GetByte(base);\r
 510         \r
 511         LOG("Realeasing Spinlock and Setting motor to stop");\r
 512         // Release Spinlock\r
 513         FDD_FreeSpinlock();\r
 514         \r
 515         //Set timer to turn off motor affter a gap\r
 516         gFDD_Devices[Disk].timer = Time_CreateTimer(MOTOR_OFF_DELAY, FDD_int_StopMotor, (void*)Disk);   //One Shot Timer
 517 \r
 518         #if USE_CACHE\r
 519         {\r
 520                 FDD_AquireCacheSpinlock();\r
 521                 int oldest = 0;\r
 522                 for(i=0;i<siFDD_SectorCacheSize;i++)\r
 523                 {\r
 524                         if(sFDD_SectorCache[i].timestamp == 0) {\r
 525                                 oldest = i;\r
 526                                 break;\r
 527                         }\r
 528                         if(sFDD_SectorCache[i].timestamp < sFDD_SectorCache[oldest].timestamp)\r
 529                                 oldest = i;\r
 530                 }\r
 531                 sFDD_SectorCache[oldest].timestamp = now();\r
 532                 sFDD_SectorCache[oldest].disk = Disk;\r
 533                 sFDD_SectorCache[oldest].sector = lba;\r
 534                 memcpy(sFDD_SectorCache[oldest].data, Buffer, 512);\r
 535                 FDD_FreeCacheSpinlock();\r
 536         }\r
 537         #else\r
 538         IOCache_Add( gFDD_Devices[Disk].CacheHandle, SectorAddr, Buffer );\r
 539         #endif\r
 540
 541         LEAVE('i', 1);\r
 542         return 1;\r
 543 }\r
 544 \r
 545 /**\r
 546  * \fn int FDD_WriteSector(Uint32 Disk, Uint64 LBA, void *Buffer)\r
 547  * \brief Write a sector to the floppy disk\r
 548  * \note Not Implemented\r
 549  */\r
 550 int FDD_WriteSector(Uint32 Disk, Uint64 LBA, void *Buffer)\r
 551 {\r
 552         Warning("[FDD  ] Read Only at the moment");\r
 553         return -1;\r
 554 }\r
 555 \r
 556 /**\r
 557  * \fn int FDD_int_SeekTrack(int disk, int track)\r
 558  * \brief Seek disk to selected track\r
 559  */\r
 560 int FDD_int_SeekTrack(int disk, int head, int track)\r
 561 {\r
 562         Uint8   sr0, cyl;\r
 563          int    base;\r
 564         \r
 565         base = cPORTBASE[disk>>1];\r
 566         \r
 567         // Check if seeking is needed\r
 568         if(gFDD_Devices[disk].track[head] == track)\r
 569                 return 1;\r
 570         \r
 571         // - Seek Head 0\r
 572         FDD_int_SendByte(base, SEEK_TRACK);\r
 573         FDD_int_SendByte(base, (head<<2)|(disk&1));\r
 574         FDD_int_SendByte(base, track);  // Send Seek command\r
 575         FDD_WaitIRQ();\r
 576         FDD_SensInt(base, &sr0, &cyl);  // Wait for IRQ\r
 577         if((sr0 & 0xF0) != 0x20) {
 578                 LOG("sr0 = 0x%x", sr0);
 579                 return 0;       //Check Status
 580         }\r
 581         if(cyl != track)        return 0;\r
 582         \r
 583         // Set Track in structure\r
 584         gFDD_Devices[disk].track[head] = track;\r
 585         return 1;\r
 586 }\r
 587 \r
 588 /**\r
 589  * \fn int FDD_int_GetDims(int type, int lba, int *c, int *h, int *s, int *spt)\r
 590  * \brief Get Dimensions of a disk\r
 591  */\r
 592 int FDD_int_GetDims(int type, int lba, int *c, int *h, int *s, int *spt)\r
 593 {\r
 594         switch(type) {\r
 595         case 0:\r
 596                 return 0;\r
 597         \r
 598         // 360Kb 5.25"\r
 599         case 1:\r
 600                 *spt = 9;\r
 601                 *s = (lba % 9) + 1;\r
 602                 *c = lba / 18;\r
 603                 *h = (lba / 9) & 1;\r
 604                 break;\r
 605         \r
 606         // 1220Kb 5.25"\r
 607         case 2:\r
 608                 *spt = 15;\r
 609                 *s = (lba % 15) + 1;\r
 610                 *c = lba / 30;\r
 611                 *h = (lba / 15) & 1;\r
 612                 break;\r
 613         \r
 614         // 720Kb 3.5"\r
 615         case 3:\r
 616                 *spt = 9;\r
 617                 *s = (lba % 9) + 1;\r
 618                 *c = lba / 18;\r
 619                 *h = (lba / 9) & 1;\r
 620                 break;\r
 621         \r
 622         // 1440Kb 3.5"\r
 623         case 4:\r
 624                 *spt = 18;\r
 625                 *s = (lba % 18) + 1;\r
 626                 *c = lba / 36;\r
 627                 *h = (lba / 18) & 1;\r
 628                 //Log("1440k - lba=%i(0x%x), *s=%i,*c=%i,*h=%i", lba, lba, *s, *c, *h);\r
 629                 break;\r
 630                 \r
 631         // 2880Kb 3.5"\r
 632         case 5:\r
 633                 *spt = 36;\r
 634                 *s = (lba % 36) + 1;\r
 635                 *c = lba / 72;\r
 636                 *h = (lba / 32) & 1;\r
 637                 break;\r
 638                 \r
 639         default:\r
 640                 return -2;\r
 641         }\r
 642         return 1;\r
 643 }\r
 644 \r
 645 /**\r
 646  * \fn void FDD_IRQHandler(int Num)\r
 647  * \brief Handles IRQ6\r
 648  */\r
 649 void FDD_IRQHandler(int Num)\r
 650 {\r
 651     fdd_irq6 = 1;\r
 652 }\r
 653 \r
 654 /**\r
 655  * \fn FDD_WaitIRQ()\r
 656  * \brief Wait for an IRQ6\r
 657  */\r
 658 void FDD_WaitIRQ()\r
 659 {\r
 660         // Wait for IRQ\r
 661         while(!fdd_irq6)        Threads_Yield();\r
 662         fdd_irq6 = 0;\r
 663 }\r
 664 \r
 665 void FDD_SensInt(int base, Uint8 *sr0, Uint8 *cyl)\r
 666 {\r
 667         FDD_int_SendByte(base, CHECK_INTERRUPT_STATUS);\r
 668         if(sr0) *sr0 = FDD_int_GetByte(base);\r
 669         else    FDD_int_GetByte(base);\r
 670         if(cyl) *cyl = FDD_int_GetByte(base);\r
 671         else    FDD_int_GetByte(base);\r
 672 }\r
 673 \r
 674 void FDD_AquireSpinlock()\r
 675 {\r
 676         while(fdd_inUse)\r
 677                 Threads_Yield();\r
 678         fdd_inUse = 1;\r
 679 }\r
 680 \r
 681 inline void FDD_FreeSpinlock()\r
 682 {\r
 683         fdd_inUse = 0;\r
 684 }\r
 685 \r
 686 #if USE_CACHE\r
 687 inline void FDD_AquireCacheSpinlock()\r
 688 {\r
 689         while(siFDD_CacheInUse) Threads_Yield();\r
 690         siFDD_CacheInUse = 1;\r
 691 }\r
 692 inline void FDD_FreeCacheSpinlock()\r
 693 {\r
 694         siFDD_CacheInUse = 0;\r
 695 }\r
 696 #endif\r
 697 \r
 698 /**\r
 699  * void FDD_int_SendByte(int base, char byte)\r
 700  * \brief Sends a command to the controller\r
 701  */\r
 702 void FDD_int_SendByte(int base, char byte)\r
 703 {\r
 704     volatile int state;\r
 705     int timeout = 128;\r
 706     for( ; timeout--; )\r
 707     {\r
 708         state = inb(base + PORT_MAINSTATUS);\r
 709         if ((state & 0xC0) == 0x80)\r
 710         {\r
 711             outb(base + PORT_DATA, byte);\r
 712             return;\r
 713         }\r
 714         inb(0x80);      //Delay\r
 715     }\r
 716         #if WARN\r
 717                 Warning("FDD_int_SendByte - Timeout sending byte 0x%x to base 0x%x\n", byte, base);\r
 718         #endif\r
 719 }\r
 720 \r
 721 /**\r
 722  * int FDD_int_GetByte(int base, char byte)\r
 723  * \brief Receive data from fdd controller\r
 724  */\r
 725 int FDD_int_GetByte(int base)\r
 726 {\r
 727     volatile int state;\r
 728     int timeout;\r
 729     for( timeout = 128; timeout--; )\r
 730     {\r
 731         state = inb((base + PORT_MAINSTATUS));\r
 732         if ((state & 0xd0) == 0xd0)\r
 733                 return inb(base + PORT_DATA);\r
 734         inb(0x80);\r
 735     }\r
 736     return -1;\r
 737 }\r
 738 \r
 739 /**\r
 740  * \brief Recalibrate the specified disk\r
 741  */\r
 742 void FDD_Recalibrate(int disk)\r
 743 {\r
 744         ENTER("idisk", disk);\r
 745         \r
 746         LOG("Starting Motor");\r
 747         FDD_int_StartMotor(disk);\r
 748         // Wait for Spinup\r
 749         while(gFDD_Devices[disk].motorState == 1)       Threads_Yield();\r
 750         \r
 751         LOG("Sending Calibrate Command");\r
 752         FDD_int_SendByte(cPORTBASE[disk>>1], CALIBRATE_DRIVE);\r
 753         FDD_int_SendByte(cPORTBASE[disk>>1], disk&1);\r
 754         \r
 755         LOG("Waiting for IRQ");\r
 756         FDD_WaitIRQ();\r
 757         FDD_SensInt(cPORTBASE[disk>>1], NULL, NULL);\r
 758         \r
 759         LOG("Stopping Motor");\r
 760         FDD_int_StopMotor(disk);\r
 761         LEAVE('-');\r
 762 }\r
 763 \r
 764 /**\r
 765  * \brief Reset the specified FDD controller\r
 766  */\r
 767 void FDD_Reset(int id)\r
 768 {\r
 769         int base = cPORTBASE[id];\r
 770         \r
 771         ENTER("iID", id);\r
 772         \r
 773         outb(base + PORT_DIGOUTPUT, 0); // Stop Motors & Disable FDC\r
 774         outb(base + PORT_DIGOUTPUT, 0x0C);      // Re-enable FDC (DMA and Enable)\r
 775         \r
 776         LOG("Awaiting IRQ");\r
 777         \r
 778         FDD_WaitIRQ();\r
 779         FDD_SensInt(base, NULL, NULL);\r
 780         \r
 781         LOG("Setting Driver Info");\r
 782         outb(base + PORT_DATARATE, 0);  // Set data rate to 500K/s\r
 783         FDD_int_SendByte(base, FIX_DRIVE_DATA); // Step and Head Load Times\r
 784         FDD_int_SendByte(base, 0xDF);   // Step Rate Time, Head Unload Time (Nibble each)\r
 785         FDD_int_SendByte(base, 0x02);   // Head Load Time >> 1\r
 786         while(FDD_int_SeekTrack(0, 0, 1) == 0); // set track\r
 787         while(FDD_int_SeekTrack(0, 1, 1) == 0); // set track\r
 788         \r
 789         LOG("Recalibrating Disk");\r
 790         FDD_Recalibrate((id<<1)|0);\r
 791         FDD_Recalibrate((id<<1)|1);
 792 \r
 793         LEAVE('-');\r
 794 }\r
 795 \r
 796 /**\r
 797  * \fn void FDD_int_TimerCallback()\r
 798  * \brief Called by timer\r
 799  */\r
 800 void FDD_int_TimerCallback(int arg)\r
 801 {\r
 802         ENTER("iarg", arg);\r
 803         if(gFDD_Devices[arg].motorState == 1)\r
 804                 gFDD_Devices[arg].motorState = 2;\r
 805         Time_RemoveTimer(gFDD_Devices[arg].timer);\r
 806         gFDD_Devices[arg].timer = -1;\r
 807         LEAVE('-');\r
 808 }\r
 809 \r
 810 /**\r
 811  * \fn void FDD_int_StartMotor(char disk)\r
 812  * \brief Starts FDD Motor\r
 813  */\r
 814 void FDD_int_StartMotor(int disk)\r
 815 {\r
 816         Uint8   state;\r
 817         state = inb( cPORTBASE[ disk>>1 ] + PORT_DIGOUTPUT );\r
 818         state |= 1 << (4+disk);\r
 819         outb( cPORTBASE[ disk>>1 ] + PORT_DIGOUTPUT, state );\r
 820         gFDD_Devices[disk].motorState = 1;\r
 821         gFDD_Devices[disk].timer = Time_CreateTimer(MOTOR_ON_DELAY, FDD_int_TimerCallback, (void*)disk);\r
 822 }\r
 823 \r
 824 /**\r
 825  * \fn void FDD_int_StopMotor(int disk)\r
 826  * \brief Stops FDD Motor\r
 827  */\r
 828 void FDD_int_StopMotor(int disk)\r
 829 {\r
 830         Uint8   state;\r
 831         state = inb( cPORTBASE[ disk>>1 ] + PORT_DIGOUTPUT );\r
 832         state &= ~( 1 << (4+disk) );\r
 833         outb( cPORTBASE[ disk>>1 ] + PORT_DIGOUTPUT, state );\r
 834     gFDD_Devices[disk].motorState = 0;\r
 835 }\r
 836 \r
 837 /**\r
 838  * \fn void ModuleUnload()\r
 839  * \brief Prepare the module for removal\r
 840  */\r
 841 void ModuleUnload()\r
 842 {\r
 843         int i;\r
 844         FDD_AquireSpinlock();\r
 845         for(i=0;i<4;i++) {\r
 846                 Time_RemoveTimer(gFDD_Devices[i].timer);\r
 847                 FDD_int_StopMotor(i);\r
 848         }\r
 849         //IRQ_Clear(6);\r
 850 }\r