Modules/Storage/FDD/fdd.c

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