Modules/Storage/FDD/fdd.c

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