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