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  * \fn Read a sector from disk\r
 412 */\r
 413 int FDD_ReadSector(Uint32 Disk, Uint64 SectorAddr, void *Buffer)\r
 414 {\r
 415          int    cyl, head, sec;\r
 416          int    spt, base;\r
 417          int    i;\r
 418          int    lba = SectorAddr;\r
 419         \r
 420         ENTER("idisk Xlba pbuf", disk, lba, buf);\r
 421         \r
 422         #if USE_CACHE\r
 423         FDD_AquireCacheSpinlock();\r
 424         for( i = 0; i < siFDD_SectorCacheSize; i++ )\r
 425         {\r
 426                 if(sFDD_SectorCache[i].timestamp == 0)  continue;\r
 427                 if(sFDD_SectorCache[i].disk == Disk\r
 428                 && sFDD_SectorCache[i].sector == lba) {\r
 429                         LOG("Found %i in cache %i", lba, i);\r
 430                         memcpy(Buffer, sFDD_SectorCache[i].data, 512);\r
 431                         sFDD_SectorCache[i].timestamp = now();\r
 432                         FDD_FreeCacheSpinlock();\r
 433                         LEAVE('i', 1);\r
 434                         return 1;\r
 435                 }\r
 436         }\r
 437         LOG("Read %i from Disk", lba);\r
 438         FDD_FreeCacheSpinlock();\r
 439         #else\r
 440         if( IOCache_Read( gFDD_Devices[Disk].CacheHandle, SectorAddr, Buffer ) == 1 ) {\r
 441                 LEAVE('i', 1);\r
 442                 return 1;\r
 443         }\r
 444         #endif\r
 445         \r
 446         base = cPORTBASE[Disk>>1];\r
 447         \r
 448         LOG("Calculating Disk Dimensions");\r
 449         // Get CHS position\r
 450         if(FDD_int_GetDims(gFDD_Devices[Disk].type, lba, &cyl, &head, &sec, &spt) != 1) {\r
 451                 LEAVE('i', -1);\r
 452                 return -1;\r
 453         }\r
 454         \r
 455         // Remove Old Timer\r
 456         Time_RemoveTimer(gFDD_Devices[Disk].timer);\r
 457         // Check if Motor is on\r
 458         if(gFDD_Devices[Disk].motorState == 0) {\r
 459                 FDD_int_StartMotor(Disk);\r
 460         }\r
 461         \r
 462         LOG("Wait for Motor Spinup");\r
 463         \r
 464         // Wait for spinup\r
 465         while(gFDD_Devices[Disk].motorState == 1)       Threads_Yield();\r
 466         \r
 467         LOG("C:%i,H:%i,S:%i", cyl, head, sec);\r
 468         LOG("Acquire Spinlock");\r
 469         \r
 470         FDD_AquireSpinlock();\r
 471         \r
 472         // Seek to track\r
 473         outb(base+CALIBRATE_DRIVE, 0);\r
 474         i = 0;\r
 475         while(FDD_int_SeekTrack(Disk, head, (Uint8)cyl) == 0 && i++ < FDD_SEEK_TIMEOUT )        Threads_Yield();\r
 476         //FDD_SensInt(base, NULL, NULL);        // Wait for IRQ\r
 477         \r
 478         LOG("Setting DMA for read");\r
 479         \r
 480         //Read Data from DMA\r
 481         DMA_SetChannel(2, 512, 1);      // Read 512 Bytes\r
 482         \r
 483         LOG("Sending read command");\r
 484         \r
 485         //Threads_Wait(100);    // Wait for Head to settle\r
 486         Time_Delay(100);\r
 487         FDD_int_SendByte(base, READ_SECTOR);    // Was 0xE6\r
 488         FDD_int_SendByte(base, (head << 2) | (Disk&1));\r
 489         FDD_int_SendByte(base, (Uint8)cyl);\r
 490         FDD_int_SendByte(base, (Uint8)head);\r
 491         FDD_int_SendByte(base, (Uint8)sec);\r
 492         FDD_int_SendByte(base, 0x02);   // Bytes Per Sector (Real BPS=128*2^{val})\r
 493         FDD_int_SendByte(base, spt);    // SPT\r
 494         FDD_int_SendByte(base, 0x1B);   // Gap Length (27 is default)\r
 495         FDD_int_SendByte(base, 0xFF);   // Data Length\r
 496         \r
 497         // Wait for IRQ\r
 498         LOG("Waiting for Data to be read");\r
 499         FDD_WaitIRQ();\r
 500         \r
 501         // Read Data from DMA\r
 502         LOG(" FDD_ReadSector: Reading Data");\r
 503         DMA_ReadData(2, 512, Buffer);\r
 504         \r
 505         // Clear Input Buffer\r
 506         LOG("Clearing Input Buffer");\r
 507         FDD_int_GetByte(base); FDD_int_GetByte(base); FDD_int_GetByte(base);\r
 508         FDD_int_GetByte(base); FDD_int_GetByte(base); FDD_int_GetByte(base); FDD_int_GetByte(base);\r
 509         \r
 510         LOG("Realeasing Spinlock and Setting motor to stop");\r
 511         // Release Spinlock\r
 512         FDD_FreeSpinlock();\r
 513         \r
 514         //Set timer to turn off motor affter a gap\r
 515         gFDD_Devices[Disk].timer = Time_CreateTimer(MOTOR_OFF_DELAY, FDD_int_StopMotor, (void*)Disk);   //One Shot Timer
 516 \r
 517         #if USE_CACHE\r
 518         {\r
 519                 FDD_AquireCacheSpinlock();\r
 520                 int oldest = 0;\r
 521                 for(i=0;i<siFDD_SectorCacheSize;i++)\r
 522                 {\r
 523                         if(sFDD_SectorCache[i].timestamp == 0) {\r
 524                                 oldest = i;\r
 525                                 break;\r
 526                         }\r
 527                         if(sFDD_SectorCache[i].timestamp < sFDD_SectorCache[oldest].timestamp)\r
 528                                 oldest = i;\r
 529                 }\r
 530                 sFDD_SectorCache[oldest].timestamp = now();\r
 531                 sFDD_SectorCache[oldest].disk = Disk;\r
 532                 sFDD_SectorCache[oldest].sector = lba;\r
 533                 memcpy(sFDD_SectorCache[oldest].data, Buffer, 512);\r
 534                 FDD_FreeCacheSpinlock();\r
 535         }\r
 536         #else\r
 537         IOCache_Add( gFDD_Devices[Disk].CacheHandle, SectorAddr, Buffer );\r
 538         #endif\r
 539
 540         LEAVE('i', 1);\r
 541         return 1;\r
 542 }\r
 543 \r
 544 /**\r
 545  * \fn int FDD_WriteSector(Uint32 Disk, Uint64 LBA, void *Buffer)\r
 546  * \brief Write a sector to the floppy disk\r
 547  * \note Not Implemented\r
 548  */\r
 549 int FDD_WriteSector(Uint32 Disk, Uint64 LBA, void *Buffer)\r
 550 {\r
 551         Warning("[FDD  ] Read Only at the moment");\r
 552         return -1;\r
 553 }\r
 554 \r
 555 /**\r
 556  * \fn int FDD_int_SeekTrack(int disk, int track)\r
 557  * \brief Seek disk to selected track\r
 558  */\r
 559 int FDD_int_SeekTrack(int disk, int head, int track)\r
 560 {\r
 561         Uint8   sr0, cyl;\r
 562          int    base;\r
 563         \r
 564         base = cPORTBASE[disk>>1];\r
 565         \r
 566         // Check if seeking is needed\r
 567         if(gFDD_Devices[disk].track[head] == track)\r
 568                 return 1;\r
 569         \r
 570         // - Seek Head 0\r
 571         FDD_int_SendByte(base, SEEK_TRACK);\r
 572         FDD_int_SendByte(base, (head<<2)|(disk&1));\r
 573         FDD_int_SendByte(base, track);  // Send Seek command\r
 574         FDD_WaitIRQ();\r
 575         FDD_SensInt(base, &sr0, &cyl);  // Wait for IRQ\r
 576         if((sr0 & 0xF0) != 0x20) {
 577                 LOG("sr0 = 0x%x", sr0);
 578                 return 0;       //Check Status
 579         }\r
 580         if(cyl != track)        return 0;\r
 581         \r
 582         // Set Track in structure\r
 583         gFDD_Devices[disk].track[head] = track;\r
 584         return 1;\r
 585 }\r
 586 \r
 587 /**\r
 588  * \fn int FDD_int_GetDims(int type, int lba, int *c, int *h, int *s, int *spt)\r
 589  * \brief Get Dimensions of a disk\r
 590  */\r
 591 int FDD_int_GetDims(int type, int lba, int *c, int *h, int *s, int *spt)\r
 592 {\r
 593         switch(type) {\r
 594         case 0:\r
 595                 return 0;\r
 596         \r
 597         // 360Kb 5.25"\r
 598         case 1:\r
 599                 *spt = 9;\r
 600                 *s = (lba % 9) + 1;\r
 601                 *c = lba / 18;\r
 602                 *h = (lba / 9) & 1;\r
 603                 break;\r
 604         \r
 605         // 1220Kb 5.25"\r
 606         case 2:\r
 607                 *spt = 15;\r
 608                 *s = (lba % 15) + 1;\r
 609                 *c = lba / 30;\r
 610                 *h = (lba / 15) & 1;\r
 611                 break;\r
 612         \r
 613         // 720Kb 3.5"\r
 614         case 3:\r
 615                 *spt = 9;\r
 616                 *s = (lba % 9) + 1;\r
 617                 *c = lba / 18;\r
 618                 *h = (lba / 9) & 1;\r
 619                 break;\r
 620         \r
 621         // 1440Kb 3.5"\r
 622         case 4:\r
 623                 *spt = 18;\r
 624                 *s = (lba % 18) + 1;\r
 625                 *c = lba / 36;\r
 626                 *h = (lba / 18) & 1;\r
 627                 //Log("1440k - lba=%i(0x%x), *s=%i,*c=%i,*h=%i", lba, lba, *s, *c, *h);\r
 628                 break;\r
 629                 \r
 630         // 2880Kb 3.5"\r
 631         case 5:\r
 632                 *spt = 36;\r
 633                 *s = (lba % 36) + 1;\r
 634                 *c = lba / 72;\r
 635                 *h = (lba / 32) & 1;\r
 636                 break;\r
 637                 \r
 638         default:\r
 639                 return -2;\r
 640         }\r
 641         return 1;\r
 642 }\r
 643 \r
 644 /**\r
 645  * \fn void FDD_IRQHandler(int Num)\r
 646  * \brief Handles IRQ6\r
 647  */\r
 648 void FDD_IRQHandler(int Num)\r
 649 {\r
 650     fdd_irq6 = 1;\r
 651 }\r
 652 \r
 653 /**\r
 654  * \fn FDD_WaitIRQ()\r
 655  * \brief Wait for an IRQ6\r
 656  */\r
 657 void FDD_WaitIRQ()\r
 658 {\r
 659         // Wait for IRQ\r
 660         while(!fdd_irq6)        Threads_Yield();\r
 661         fdd_irq6 = 0;\r
 662 }\r
 663 \r
 664 void FDD_SensInt(int base, Uint8 *sr0, Uint8 *cyl)\r
 665 {\r
 666         FDD_int_SendByte(base, CHECK_INTERRUPT_STATUS);\r
 667         if(sr0) *sr0 = FDD_int_GetByte(base);\r
 668         else    FDD_int_GetByte(base);\r
 669         if(cyl) *cyl = FDD_int_GetByte(base);\r
 670         else    FDD_int_GetByte(base);\r
 671 }\r
 672 \r
 673 void FDD_AquireSpinlock()\r
 674 {\r
 675         while(fdd_inUse)\r
 676                 Threads_Yield();\r
 677         fdd_inUse = 1;\r
 678 }\r
 679 \r
 680 inline void FDD_FreeSpinlock()\r
 681 {\r
 682         fdd_inUse = 0;\r
 683 }\r
 684 \r
 685 #if USE_CACHE\r
 686 inline void FDD_AquireCacheSpinlock()\r
 687 {\r
 688         while(siFDD_CacheInUse) Threads_Yield();\r
 689         siFDD_CacheInUse = 1;\r
 690 }\r
 691 inline void FDD_FreeCacheSpinlock()\r
 692 {\r
 693         siFDD_CacheInUse = 0;\r
 694 }\r
 695 #endif\r
 696 \r
 697 /**\r
 698  * void FDD_int_SendByte(int base, char byte)\r
 699  * \brief Sends a command to the controller\r
 700  */\r
 701 void FDD_int_SendByte(int base, char byte)\r
 702 {\r
 703     volatile int state;\r
 704     int timeout = 128;\r
 705     for( ; timeout--; )\r
 706     {\r
 707         state = inb(base + PORT_MAINSTATUS);\r
 708         if ((state & 0xC0) == 0x80)\r
 709         {\r
 710             outb(base + PORT_DATA, byte);\r
 711             return;\r
 712         }\r
 713         inb(0x80);      //Delay\r
 714     }\r
 715         #if WARN\r
 716                 Warning("FDD_int_SendByte - Timeout sending byte 0x%x to base 0x%x\n", byte, base);\r
 717         #endif\r
 718 }\r
 719 \r
 720 /**\r
 721  * int FDD_int_GetByte(int base, char byte)\r
 722  * \brief Receive data from fdd controller\r
 723  */\r
 724 int FDD_int_GetByte(int base)\r
 725 {\r
 726     volatile int state;\r
 727     int timeout;\r
 728     for( timeout = 128; timeout--; )\r
 729     {\r
 730         state = inb((base + PORT_MAINSTATUS));\r
 731         if ((state & 0xd0) == 0xd0)\r
 732                 return inb(base + PORT_DATA);\r
 733         inb(0x80);\r
 734     }\r
 735     return -1;\r
 736 }\r
 737 \r
 738 /**\r
 739  * \brief Recalibrate the specified disk\r
 740  */\r
 741 void FDD_Recalibrate(int disk)\r
 742 {\r
 743         ENTER("idisk", disk);\r
 744         \r
 745         LOG("Starting Motor");\r
 746         FDD_int_StartMotor(disk);\r
 747         // Wait for Spinup\r
 748         while(gFDD_Devices[disk].motorState == 1)       Threads_Yield();\r
 749         \r
 750         LOG("Sending Calibrate Command");\r
 751         FDD_int_SendByte(cPORTBASE[disk>>1], CALIBRATE_DRIVE);\r
 752         FDD_int_SendByte(cPORTBASE[disk>>1], disk&1);\r
 753         \r
 754         LOG("Waiting for IRQ");\r
 755         FDD_WaitIRQ();\r
 756         FDD_SensInt(cPORTBASE[disk>>1], NULL, NULL);\r
 757         \r
 758         LOG("Stopping Motor");\r
 759         FDD_int_StopMotor(disk);\r
 760         LEAVE('-');\r
 761 }\r
 762 \r
 763 /**\r
 764  * \brief Reset the specified FDD controller\r
 765  */\r
 766 void FDD_Reset(int id)\r
 767 {\r
 768         int base = cPORTBASE[id];\r
 769         \r
 770         ENTER("iID", id);\r
 771         \r
 772         outb(base + PORT_DIGOUTPUT, 0); // Stop Motors & Disable FDC\r
 773         outb(base + PORT_DIGOUTPUT, 0x0C);      // Re-enable FDC (DMA and Enable)\r
 774         \r
 775         LOG("Awaiting IRQ");\r
 776         \r
 777         FDD_WaitIRQ();\r
 778         FDD_SensInt(base, NULL, NULL);\r
 779         \r
 780         LOG("Setting Driver Info");\r
 781         outb(base + PORT_DATARATE, 0);  // Set data rate to 500K/s\r
 782         FDD_int_SendByte(base, FIX_DRIVE_DATA); // Step and Head Load Times\r
 783         FDD_int_SendByte(base, 0xDF);   // Step Rate Time, Head Unload Time (Nibble each)\r
 784         FDD_int_SendByte(base, 0x02);   // Head Load Time >> 1\r
 785         while(FDD_int_SeekTrack(0, 0, 1) == 0); // set track\r
 786         while(FDD_int_SeekTrack(0, 1, 1) == 0); // set track\r
 787         \r
 788         LOG("Recalibrating Disk");\r
 789         FDD_Recalibrate((id<<1)|0);\r
 790         FDD_Recalibrate((id<<1)|1);
 791 \r
 792         LEAVE('-');\r
 793 }\r
 794 \r
 795 /**\r
 796  * \fn void FDD_int_TimerCallback()\r
 797  * \brief Called by timer\r
 798  */\r
 799 void FDD_int_TimerCallback(int arg)\r
 800 {\r
 801         ENTER("iarg", arg);\r
 802         if(gFDD_Devices[arg].motorState == 1)\r
 803                 gFDD_Devices[arg].motorState = 2;\r
 804         Time_RemoveTimer(gFDD_Devices[arg].timer);\r
 805         gFDD_Devices[arg].timer = -1;\r
 806         LEAVE('-');\r
 807 }\r
 808 \r
 809 /**\r
 810  * \fn void FDD_int_StartMotor(char disk)\r
 811  * \brief Starts FDD Motor\r
 812  */\r
 813 void FDD_int_StartMotor(int disk)\r
 814 {\r
 815         Uint8   state;\r
 816         state = inb( cPORTBASE[ disk>>1 ] + PORT_DIGOUTPUT );\r
 817         state |= 1 << (4+disk);\r
 818         outb( cPORTBASE[ disk>>1 ] + PORT_DIGOUTPUT, state );\r
 819         gFDD_Devices[disk].motorState = 1;\r
 820         gFDD_Devices[disk].timer = Time_CreateTimer(MOTOR_ON_DELAY, FDD_int_TimerCallback, (void*)disk);\r
 821 }\r
 822 \r
 823 /**\r
 824  * \fn void FDD_int_StopMotor(int disk)\r
 825  * \brief Stops FDD Motor\r
 826  */\r
 827 void FDD_int_StopMotor(int disk)\r
 828 {\r
 829         Uint8   state;\r
 830         state = inb( cPORTBASE[ disk>>1 ] + PORT_DIGOUTPUT );\r
 831         state &= ~( 1 << (4+disk) );\r
 832         outb( cPORTBASE[ disk>>1 ] + PORT_DIGOUTPUT, state );\r
 833     gFDD_Devices[disk].motorState = 0;\r
 834 }\r
 835 \r
 836 /**\r
 837  * \fn void ModuleUnload()\r
 838  * \brief Prepare the module for removal\r
 839  */\r
 840 void ModuleUnload()\r
 841 {\r
 842         int i;\r
 843         FDD_AquireSpinlock();\r
 844         for(i=0;i<4;i++) {\r
 845                 Time_RemoveTimer(gFDD_Devices[i].timer);\r
 846                 FDD_int_StopMotor(i);\r
 847         }\r
 848         //IRQ_Clear(6);\r
 849 }\r