Modules/Storage/FDD/fdd.c

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