drivers/block/.svn/text-base/floppy.c.svn-base

   1 #include <system.h>
   2 #include <x86.h>
   3 #include <string.h>
   4
   5
   6 /* FDC_H */
   7 /* globals */
   8 static volatile BOOL done = FALSE;
   9 static BOOL dchange = FALSE;
  10 static BOOL motor = FALSE;
  11 static int mtick = 0;
  12 static volatile int tmout = 0;
  13 static BYTE status[7] = { 0 };
  14 static BYTE statsz = 0;
  15 static BYTE sr0 = 0;
  16 static BYTE fdc_track = 0xff;
  17 static DrvGeom geometry = { DG144_HEADS,DG144_TRACKS,DG144_SPT };
  18
  19 unsigned long tbaddr = 0x80000L;    /* physical address of track buffer located below 1M */
  20
  21 /* prototypes */
  22 extern void floppy_ISR();
  23 extern void _int1c();
  24
  25 void sendbyte(int byte);
  26 int getbyte();
  27 void floppy_handler(struct regs *r);
  28 void int1c(void);
  29 BOOL waitfdc(BOOL sensei);
  30 BOOL fdc_rw(int block,BYTE *blockbuff,BOOL read,unsigned long nosectors);
  31
  32 /* helper functions */
  33
  34 void floppy_detect(void)
  35 {
  36         unsigned char c;
  37         int a,b;
  38         char *drive_type[5] = { "no floppy drive", "360kb 5.25in floppy drive", "1.2mb 5.25in floppy drive", "720kb3.5in", "1.44mb 3.5in", "2.88mb 3.5in"};
  39         outportb(0x70, 0x10);
  40         c = inportb(0x71);
  41         a = c >> 4;
  42         b = c & 0xF;
  43         kprintf("Floppy drive A: %s\n",drive_type[a]);
  44         kprintf("Floppy drive B: %s\n",drive_type[b]);
  45 }
  46
  47
  48 /* This is the IRQ6 handler */
  49 void floppy_handler(struct regs *r)
  50 {
  51         /* signal operation finished */
  52         done = TRUE;
  53         /* EOI the PIC */
  54         //outportb(0x20,0x20);
  55 }
  56
  57 /* init driver */
  58 void floppy_install(void)
  59 {
  60         irq_install_handler(6, floppy_handler);
  61 }
  62
  63 void init_floppy(void)
  64 {
  65         int i;
  66
  67         /* allocate track buffer (must be located below 1M) */
  68         /* see above for address assignment, floppy  buffer is at 0x80000) */
  69         /* install IRQ6 handler */
  70
  71         done=FALSE;
  72
  73         reset();
  74
  75         /* get floppy controller version */
  76         sendbyte(CMD_VERSION);
  77         i = getbyte();
  78
  79         if (i == 0x80)
  80                 kprintf("NEC765 controller found\n");
  81         else
  82                 kprintf("Enhanced controller found\n");
  83 }
  84
  85 /* sendbyte() routine from intel manual */
  86 void sendbyte(int byte)
  87 {
  88         volatile int msr;
  89         int tmo;
  90         for (tmo = 0;tmo < 128;tmo++)
  91         {
  92                 msr = inportb(FDC_MSR);
  93                 if ((msr & 0xc0) == 0x80)
  94                 {
  95                         outportb(FDC_DATA,byte);
  96                         return;
  97                 }
  98                 inportb(0x80);   /* delay */
  99         }
 100 }
 101
 102 /* getbyte() routine from intel manual */
 103 int getbyte()
 104 {
 105         volatile int msr;
 106         int tmo;
 107
 108         for (tmo = 0;tmo < 128;tmo++)
 109         {
 110                 msr = inportb(FDC_MSR);
 111                 if ((msr & 0xd0) == 0xd0)
 112                 {
 113                         return inportb(FDC_DATA);
 114                 }
 115                 inportb(0x80);   /* delay */
 116         }
 117         return -1;   /* read timeout */
 118 }
 119
 120 /* this waits for FDC command to complete */
 121 BOOL waitfdc(BOOL sensei)
 122 {
 123         tmout=80000;
 124         /* wait for IRQ6 handler to signal command finished */
 125         while (!done && tmout)
 126                 kprintf("");;
 127
 128         /* read in command result bytes */
 129         statsz = 0;
 130         while ((statsz < 7) && (inportb(FDC_MSR) & 0x10))
 131         {
 132                 status[statsz++] = getbyte();
 133         }
 134
 135         if (sensei)
 136         {
 137                 /* send a "sense interrupt status" command */
 138                 sendbyte(CMD_SENSEI);
 139                 sr0 = getbyte();
 140                 fdc_track = getbyte();
 141         }
 142
 143         done = FALSE;
 144 /*
 145         if (!tmout)
 146         {
 147                 if (inportb(FDC_DIR) & 0x80)
 148                         dchange = TRUE;
 149                 return FALSE;
 150         }
 151         else*/
 152                 return TRUE;
 153 }
 154
 155
 156
 157 /* This is the timer (int 1ch) handler */
 158 void int1c(void)
 159 {
 160         if (tmout) --tmout;     /* bump timeout */
 161         if (mtick > 0)
 162                 --mtick;
 163         else if (!mtick && motor)
 164         {
 165                 outportb(FDC_DOR,0x0c);  /* turn off floppy motor */
 166                 motor = FALSE;
 167         }
 168 }
 169
 170 /*
 171  * converts linear block address to head/track/sector
 172  *
 173  * blocks are numbered 0..heads*tracks*spt-1
 174  * blocks 0..spt-1 are serviced by head #0
 175  * blocks spt..spt*2-1 are serviced by head 1
 176  *
 177  * WARNING: garbage in == garbage out
 178  */
 179 void block2hts(int block,int *head,int *track,int *sector)
 180 {
 181         *head = (block % (geometry.spt * geometry.heads)) / (geometry.spt);
 182         *track = block / (geometry.spt * geometry.heads);
 183         *sector = block % geometry.spt + 1;
 184 }
 185
 186 /**** disk operations ****/
 187
 188 /* this gets the FDC to a known state */
 189 void reset(void)
 190 {
 191         /* stop the motor and disable IRQ*/
 192         motoroff();
 193         outportb(FDC_DOR,0);
 194         mtick = 0;
 195         motor = FALSE;
 196
 197         /* program data rate (500K/s) */
 198         outportb(FDC_DRS,0);
 199
 200         /* re-enable interrupts */
 201         outportb(FDC_DOR,0x0c);
 202
 203         /* resetting triggered an interrupt - handle it */
 204         done = TRUE;
 205         waitfdc(TRUE);
 206         done = TRUE;
 207         waitfdc(TRUE);
 208         done = TRUE;
 209         waitfdc(TRUE);
 210         done = TRUE;
 211         waitfdc(TRUE);
 212
 213         /* specify drive timings (got these off the BIOS) */
 214
 215         sendbyte(CMD_SPECIFY);
 216         sendbyte(0xdf);  /* SRT = 3ms, HUT = 240ms */
 217         sendbyte(0x02);  /* HLT = 16ms, ND = 0 */
 218         /* clear "disk change" status */
 219         //kprintf("RESET|SEEK\n");
 220         seek(5);
 221
 222         recalibrate();
 223         dchange = FALSE;
 224 }
 225
 226 /* this returns whether there was a disk change */
 227 BOOL diskchange(void)
 228 {
 229         return dchange;
 230 }
 231
 232 /* this turns the motor on */
 233 void motoron(void)
 234 {
 235         if (!motor)
 236         {
 237                 mtick = -1;     /* stop motor kill countdown */
 238                 outportb(FDC_DOR,0x1c);
 239
 240                 timer_wait(9); /* delay 500ms for motor to spin up */
 241                 motor = TRUE;
 242         }
 243 }
 244
 245 /* this turns the motor off */
 246 void motoroff(void)
 247 {
 248         if (motor)
 249         {
 250 //              mtick = 13500;   /* start motor kill countdown: 36 ticks ~ 2s */
 251
 252                 outportb(FDC_DOR,0x0c);
 253                 outportb(0x3f2,0);
 254                 motor=FALSE;
 255                 timer_wait(36);
 256         }
 257 }
 258
 259 /* recalibrate the drive */
 260 void recalibrate(void)
 261 {
 262         /* turn the motor on */
 263         //motoron();
 264
 265         /* send actual command bytes */
 266         sendbyte(CMD_RECAL);
 267         sendbyte(0);
 268         /* wait until seek finished */
 269         waitfdc(TRUE);
 270
 271 /* turn the motor off */
 272         //motoroff();
 273
 274 }
 275
 276 /* seek to track */
 277 BOOL seek(int track)
 278 {
 279
 280         if (fdc_track == track)  /* already there? */
 281                 return TRUE;
 282
 283         //motoron();
 284         /* send actual command bytes */
 285         sendbyte(CMD_SEEK);
 286         sendbyte(0);
 287         sendbyte(track);
 288         /* wait until seek finished */
 289         if (!waitfdc(TRUE))
 290         {
 291                 kprintf("FLOPPY | SEEK | TIMEOUT\n");
 292                 return FALSE;     /* timeout! */
 293         }
 294         /* now let head settle for 15ms */
 295         timer_wait(5);
 296         //motoroff();
 297         /* check that seek worked */
 298         if ((sr0 != 0x20) || (fdc_track != track))
 299         {
 300                 if (sr0 != 0x20) kprintf("FLOPPY | SEEK | SR0!=0x20\n");
 301                 if (fdc_track != track) kprintf("FLOPPY | SEEK | FDC_TRACK!=TRACK\n");
 302                 return FALSE;
 303         }
 304         else
 305                 return TRUE;
 306 }
 307
 308 /* checks drive geometry - call this after any disk change */
 309 BOOL log_disk(DrvGeom *g)
 310 {
 311         /* get drive in a known status before we do anything */
 312         reset();
 313
 314         /* assume disk is 1.68M and try and read block #21 on first track */
 315         geometry.heads = DG168_HEADS;
 316         geometry.tracks = DG168_TRACKS;
 317         geometry.spt = DG168_SPT;
 318
 319         if (read_block(20,NULL,1))
 320         {
 321                 /* disk is a 1.68M disk */
 322                 if (g)
 323                 {
 324                         g->heads = geometry.heads;
 325                         g->tracks = geometry.tracks;
 326                         g->spt = geometry.spt;
 327                 }
 328                 return TRUE;
 329         }
 330
 331         /* OK, not 1.68M - try again for 1.44M reading block #18 on first track */
 332         geometry.heads = DG144_HEADS;
 333         geometry.tracks = DG144_TRACKS;
 334         geometry.spt = DG144_SPT;
 335
 336         if (read_block(17,NULL,1))
 337         {
 338                 /* disk is a 1.44M disk */
 339                 if (g)
 340                 {
 341                         g->heads = geometry.heads;
 342                         g->tracks = geometry.tracks;
 343                         g->spt = geometry.spt;
 344                 }
 345                 return TRUE;
 346         }
 347
 348         /* it's not 1.44M or 1.68M - we don't support it */
 349         return FALSE;
 350 }
 351
 352 /* read block (blockbuff is 512 byte buffer) */
 353 BOOL read_block(int block,BYTE *blockbuff, unsigned long nosectors)
 354 {
 355         int track=0, sector=0, head=0, track2=0, result=0, loop=0;
 356         reset();
 357         //The FDC can read multiple sides at once but not multiple tracks
 358         //kprintf("READ 1\n");
 359         block2hts(block, &head, &track, &sector);
 360         block2hts(block+nosectors, &head, &track2, &sector);
 361
 362         if(track!=track2)
 363         {
 364                 for(loop=0; (unsigned)loop<nosectors; loop++)
 365                         result = fdc_rw(block+loop, blockbuff+(loop*512), TRUE, 1);
 366                 return result;
 367         }
 368 //      kprintf("FDC_RW\n");
 369         return fdc_rw(block,blockbuff,TRUE,nosectors);
 370 }
 371
 372 /* write block (blockbuff is 512 byte buffer) */
 373 BOOL write_block(int block,BYTE *blockbuff, unsigned long nosectors)
 374 {
 375         return fdc_rw(block,blockbuff,FALSE, nosectors);
 376 }
 377
 378 /*
 379  * since reads and writes differ only by a few lines, this handles both.  This
 380  * function is called by read_block() and write_block()
 381  */
 382 BOOL fdc_rw(int block,BYTE *blockbuff,BOOL read,unsigned long nosectors)
 383 {
 384         int head,track,sector,tries, copycount = 0;
 385         unsigned char *p_tbaddr = (unsigned char *)0x80000;
 386         unsigned char *p_blockbuff = blockbuff;
 387
 388         /* convert logical address into physical address */
 389         block2hts(block,&head,&track,&sector);
 390
 391         /* spin up the disk */
 392         kprintf("FDC_RW|MOTORON\n");
 393         motoron();
 394
 395         if (!read && blockbuff)
 396         {
 397                 /* copy data from data buffer into track buffer */
 398                 for(copycount=0; (unsigned)copycount<(nosectors*512); copycount++)
 399                 {
 400                         *p_tbaddr = *p_blockbuff;
 401                         p_blockbuff++;
 402                         p_tbaddr++;
 403                 }
 404         }
 405         kprintf("FDC_RW|COPY1\n");
 406         for (tries = 0;tries < 3;tries++)
 407         {
 408
 409                 kprintf("FDC_RW|RIGHT\n");
 410                 /* move head to right track */
 411                 if (!seek(track))
 412                 {
 413                         motoroff();
 414                         //kprintf("FLOPPY | CANNOT SEEK\n");
 415                         return FALSE;
 416                 }
 417
 418                 /* check for diskchange */
 419                 kprintf("FDC_RW|DISKCHANGE\n");
 420                 if (inportb(FDC_DIR) & 0x80)
 421                 {
 422                         dchange = TRUE;
 423                         seek(1);  /* clear "disk change" status */
 424                         recalibrate();
 425                         motoroff();
 426                         //kprintf("FLOPPY | DISK CHANGE STATUS => repeat\n");
 427                         return fdc_rw(block, blockbuff, read, nosectors);
 428                 }
 429
 430
 431                 kprintf("FDC_RW|RATE\n");
 432                 /* program data rate (500K/s) */
 433                 outportb(FDC_CCR,0);
 434                 /* send command */
 435
 436                 if (read)
 437                 {
 438                         kprintf("FDC_RW|DMA\n");
 439                         dma_xfer(2,tbaddr,nosectors*512,FALSE);
 440                         sendbyte(CMD_READ);
 441                 }
 442                 else
 443                 {
 444                         dma_xfer(2,tbaddr,nosectors*512,TRUE);
 445                         sendbyte(CMD_WRITE);
 446                 }
 447                 sendbyte(head << 2);
 448                 sendbyte(track);
 449                 sendbyte(head);
 450                 sendbyte(sector);
 451                 sendbyte(2);               /* 512 bytes/sector */
 452                 sendbyte(geometry.spt);
 453                 if (geometry.spt == DG144_SPT)
 454                         sendbyte(DG144_GAP3RW);  /* gap 3 size for 1.44M read/write */
 455                 else
 456                         sendbyte(DG168_GAP3RW);  /* gap 3 size for 1.68M read/write */
 457                 sendbyte(0xff);            /* DTL = unused */
 458
 459                 kprintf("FDC_RW|STEP1\n");
 460                 /* wait for command completion */
 461                 /* read/write don't need "sense interrupt status" */
 462                 if (!waitfdc(1))
 463                 {
 464                         kprintf("Timed out, trying operation again after reset()\n");
 465                         reset();
 466                         return fdc_rw(block, blockbuff, read, nosectors);
 467         }
 468                 kprintf("FDC_RW|%d\n",(status[0] & 0xc0));
 469                 if ((status[0] & 0xc0) == 0){ break;}   /* worked! outta here! */
 470                 //break;
 471                 kprintf("Try %d error => read from floppy\n",tries);
 472                 recalibrate();  /* oops, try again... */
 473         }
 474
 475
 476         /* stop the motor */
 477         //kprintf("FDC_RW|MOTOROFF\n");
 478         motoroff();
 479
 480         if (read && blockbuff)
 481         {
 482                 /* copy data from track buffer into data buffer */
 483                 //kprintf("FDC_RW|OK\n");
 484                 p_blockbuff = blockbuff;
 485                 p_tbaddr = (unsigned char *) 0x80000;
 486                 for(copycount=0; (unsigned)copycount<(nosectors*512); copycount++)
 487                 {
 488                         *p_blockbuff = *p_tbaddr;
 489                         p_blockbuff++;
 490                         p_tbaddr++;
 491                 }
 492         }
 493         //else{kprintf("FLOPPY | ERROR\n");}
 494         return (tries != 3);
 495 }
 496
 497 /* this formats a track, given a certain geometry */
 498 BOOL format_track(BYTE track,DrvGeom *g)
 499 {
 500         int i,h,r,r_id,split;
 501         BYTE tmpbuff[256];
 502         unsigned char *p_tbaddr = (unsigned char *)0x8000;
 503         unsigned int copycount = 0;
 504
 505         /* check geometry */
 506         if (g->spt != DG144_SPT && g->spt != DG168_SPT)
 507                 return FALSE;
 508
 509         /* spin up the disk */
 510         motoron();
 511
 512         /* program data rate (500K/s) */
 513         outportb(FDC_CCR,0);
 514
 515         seek(track);  /* seek to track */
 516
 517         /* precalc some constants for interleave calculation */
 518         split = g->spt / 2;
 519         if (g->spt & 1) split++;
 520         for (h = 0;h < g->heads;h++)
 521         {
 522                 /* for each head... */
 523                 /* check for diskchange */
 524                 if (inportb(FDC_DIR) & 0x80)
 525                 {
 526                         dchange = TRUE;
 527                         seek(1);  /* clear "disk change" status */
 528                         recalibrate();
 529                         motoroff();
 530                         return FALSE;
 531                 }
 532
 533                 i = 0;   /* reset buffer index */
 534                 for (r = 0;r < g->spt;r++)
 535                 {
 536                         /* for each sector... */
 537
 538                         /* calculate 1:2 interleave (seems optimal in my system) */
 539                         r_id = r / 2 + 1;
 540                         if (r & 1) r_id += split;
 541
 542                         /* add some head skew (2 sectors should be enough) */
 543                         if (h & 1)
 544                         {
 545                                 r_id -= 2;
 546                                 if (r_id < 1) r_id += g->spt;
 547                         }
 548                         /* add some track skew (1/2 a revolution) */
 549                         if (track & 1)
 550                         {
 551                                 r_id -= g->spt / 2;
 552                                 if (r_id < 1) r_id += g->spt;
 553                         }
 554
 555                         /**** interleave now calculated - sector ID is stored in r_id ****/
 556
 557                         /* fill in sector ID's */
 558                         tmpbuff[i++] = track;
 559                         tmpbuff[i++] = h;
 560                         tmpbuff[i++] = r_id;
 561                         tmpbuff[i++] = 2;
 562                 }
 563
 564                 /* copy sector ID's to track buffer */
 565                 for(copycount = 0; copycount<(unsigned)i; copycount++)
 566                 {
 567                         *p_tbaddr = tmpbuff[copycount];
 568                         p_tbaddr++;
 569                 }
 570                 //movedata(_my_ds(),(long)tmpbuff,_dos_ds,tbaddr,i);
 571
 572                 /* start dma xfer */
 573                 dma_xfer(2,tbaddr,i,TRUE);
 574
 575                 /* prepare "format track" command */
 576                 sendbyte(CMD_FORMAT);
 577                 sendbyte(h << 2);
 578                 sendbyte(2);
 579                 sendbyte(g->spt);
 580                 if (g->spt == DG144_SPT)
 581                         sendbyte(DG144_GAP3FMT);    /* gap3 size for 1.44M format */
 582                 else
 583                         sendbyte(DG168_GAP3FMT);    /* gap3 size for 1.68M format */
 584                 sendbyte(0);     /* filler byte */
 585
 586                 /* wait for command to finish */
 587                 if (!waitfdc(FALSE))
 588                 return FALSE;
 589                 if (status[0] & 0xc0)
 590                 {
 591                         motoroff();
 592                         return FALSE;
 593                 }
 594         }
 595         motoroff();
 596         return TRUE;
 597 }
 598
 599 //void floppy_install()
 600 //{
 601     /* Installs 'timer_handler' to IRQ0 */
 602     //irq_install_handler(6, floppy_handler);
 603 //}
 604
 605 asm (
 606    ".globl _int1c        \n"
 607    "_int1c:              \n"
 608    "   pusha               \n" /* Save all registers               */
 609    "   pushw %ds           \n" /* Set up the data segment          */
 610    "   pushw %es           \n"
 611    "   pushw %ss           \n" /* Note that ss is always valid     */
 612    "   pushw %ss           \n"
 613    "   popw %ds            \n"
 614    "   popw %es            \n"
 615    "                       \n"
 616    "   call int1c          \n"
 617    "                       \n"
 618    "   popw %es            \n"
 619    "   popw %ds            \n" /* Restore registers                */
 620    "   popa                \n"
 621    "   iret                \n" /* Exit interrupt                   */
 622 );
 623
 624 asm (
 625    ".globl floppy_ISR        \n"
 626    "floppy_ISR:              \n"
 627    "   pusha               \n" /* Save all registers               */
 628    "   pushw %ds           \n" /* Set up the data segment          */
 629    "   pushw %es           \n"
 630    "   pushw %ss           \n" /* Note that ss is always valid     */
 631    "   pushw %ss           \n"
 632    "   popw %ds            \n"
 633    "   popw %es            \n"
 634    "                       \n"
 635    "   call irq6           \n"
 636    "                       \n"
 637    "   popw %es            \n"
 638    "   popw %ds            \n" /* Restore registers                */
 639    "   popa                \n"
 640    "   iret                \n" /* Exit interrupt                   */
 641 );