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

UCC git Repository :: git.ucc.asn.au