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