rom/devs/trackdisk/trackdisk_hw.c

   1 /*
   2     Copyright © 1995-2008, The AROS Development Team. All rights reserved.
   3     $Id$
   4
   5     Desc: Intel HW floppy stuff
   6     Lang: English
   7 */
   8
   9 #include <exec/types.h>
  10 #include <devices/trackdisk.h>
  11
  12 #include "trackdisk_device.h"
  13 #include "trackdisk_hw.h"
  14
  15 #define DEBUG 0
  16 #include <aros/debug.h>
  17
  18 #undef kprintf
  19
  20 #define ioStd(x)  ((struct IOStdReq *)x)
  21
  22 /*-------------------- Hardware section --------------------*/
  23
  24 /* All procedures here are based on Intel documentation */
  25
  26 #define TD_DMA  2               /* Define DMA channel */
  27
  28 /* Wait for status flags to settle */
  29 int td_waitUntilReady(void)
  30 {
  31     int i, status;
  32
  33     for (i=0; i < 10000; i++)
  34     {
  35         status = inb(FDC_MSR);
  36         if (status & MSRF_RQM)
  37             return status;
  38     }
  39     return -1;
  40 }
  41
  42
  43 /* Start motor and select drive */
  44 void td_motoron(UBYTE unitnum, struct TrackDiskBase *tdb, BOOL wait)
  45 {
  46     UBYTE dor;
  47
  48     dor = tdb->td_dor & (DORF_MOT0|DORF_MOT1);
  49     dor |= DORF_DMA | DORF_RESET | (DORF_MOT0 << unitnum) | unitnum;
  50     tdb->td_dor = dor;
  51
  52     outb(dor,FDC_DOR);
  53     /* Did the user want to wait for spinup? */
  54     if (wait)
  55     {
  56         td_waitUntilReady();
  57     }
  58 }
  59
  60 /* Stop motor */
  61 void td_motoroff(UBYTE unitnum, struct TrackDiskBase *tdb)
  62 {
  63     tdb->td_dor = tdb->td_dor & ~(DORF_MOT0 << unitnum);
  64     outb(tdb->td_dor,FDC_DOR);
  65 }
  66
  67 // Send byte to drive. Returns DriveInUse error if busy, 0 otherwise
  68 int td_sendbyte(unsigned char byte, struct TrackDiskBase *TDBase)
  69 {
  70     int i;
  71
  72     for(i=0;i<100000;i++)
  73     {
  74         if ((inb(FDC_MSR) & (MSRF_RQM | MSRF_DIO))==MSRF_RQM)
  75         {
  76             outb(byte, FDC_FIFO);
  77             return 0;
  78         }
  79     }
  80     D(bug("TD: sendbyte failed\n"));
  81     return TDERR_DriveInUse;
  82
  83 }
  84
  85 // Get byte from drive. Returns the same as td_sendbyte
  86 int td_getbyte(unsigned char *byte, struct TrackDiskBase *TDBase)
  87 {
  88     int i;
  89
  90     for(i=0;i<100000;i++)
  91     {
  92         if ((inb(FDC_MSR) & (MSRF_RQM | MSRF_DIO))==(MSRF_RQM | MSRF_DIO))
  93         {
  94             *byte = inb(FDC_FIFO);
  95             return 0;
  96         }
  97     }
  98     return TDERR_DriveInUse;
  99 }
 100
 101 // Send full command to drive
 102 int td_sendcommand(struct TrackDiskBase *TDBase)
 103 {
 104     int err = 0;
 105
 106     if (TDBase->td_comsize)
 107     {
 108         int i;
 109         for (i=0; (i < TDBase->td_comsize) && !err; i++)
 110         {
 111             err = td_sendbyte(TDBase->td_rawcom[i], TDBase);
 112         }
 113     }
 114     // If there was error just reset drive
 115     if (err)
 116     {
 117         int i;
 118
 119         D(bug("[Floppy] Resending command\n"));
 120         err = 0;
 121         td_dinit(TDBase);
 122         // Resend command
 123         for (i=0; (i < TDBase->td_comsize) && !err; i++)
 124             err = td_sendbyte(TDBase->td_rawcom[i], TDBase);
 125     }
 126     return err;
 127 }
 128
 129 int td_sendcommand2(struct TrackDiskBase *TDBase)
 130 {
 131     int err = 0;
 132
 133     if (TDBase->td_comsize2)
 134     {
 135         int i;
 136         for (i=0; (i < TDBase->td_comsize2) && !err; i++)
 137         {
 138             err = td_sendbyte(TDBase->td_rawcom2[i], TDBase);
 139         }
 140     }
 141     return err;
 142 }
 143
 144 /* Wait for interrupt */
 145 int td_waitint(struct TrackDiskBase *TDBase, UBYTE results,BOOL sensei)
 146 {
 147     int i;
 148     ULONG sigs;
 149
 150     TDBase->td_inttmo = 150;    // Each IO command has 1s to complete before error occurs
 151     sigs = Wait((1L << TDBase->td_IntBit)|(1L<<TDBase->td_TmoBit));
 152     if (sigs & (1L << TDBase->td_IntBit))
 153     {
 154         TDBase->td_inttmo = 0;
 155         if (results)
 156         {
 157             i = 0;
 158             while ( (i<results) && (inb(FDC_MSR) & MSRF_CMDBSY) )
 159             {
 160                 td_getbyte(&TDBase->td_result[i],TDBase);
 161             }
 162         }
 163         /* Do we run a Sense Interrupt? */
 164         if (sensei)
 165         {
 166             td_sendbyte(FD_SENSEI,TDBase);
 167             td_getbyte(&TDBase->td_sr0,TDBase);
 168             td_getbyte(&TDBase->td_pcn,TDBase);
 169         }
 170         return 0;
 171     }
 172     return TDERR_NotSpecified;
 173 }
 174
 175 // Read status bytes
 176 int td_readstatus(struct TrackDiskBase *TDBase, int num)
 177 {
 178     int i, err = 0;
 179     if (num > 7) num = 7;
 180     for (i=0; (i < num) && !err; i++)
 181     {
 182         err = td_getbyte(&TDBase->td_result[i], TDBase);
 183     }
 184     return err;
 185 }
 186
 187 #define MORE_OUTPUT 1
 188
 189 int needMoreOutput(struct TrackDiskBase *TDBase)
 190 {
 191     int status;
 192
 193     if ((status=td_waitUntilReady())<0)
 194         return -1;
 195     if ((status & (MSRF_RQM|MSRF_DIO|MSRF_NONDMA)) == MSRF_RQM)
 196         return MORE_OUTPUT;
 197     td_readstatus(TDBase, 7);
 198     return 0;
 199 }
 200
 201 int td_configure(struct TrackDiskBase *TDBase)
 202 {
 203
 204     // Do Configure (enable FIFO and turn polling off)
 205     // We also enable implied seeks
 206     td_sendbyte(FD_CONFIGURE, TDBase);
 207     if (needMoreOutput(TDBase) == MORE_OUTPUT)
 208     {
 209         td_sendbyte(0, TDBase);
 210         td_sendbyte(0x1a, TDBase);
 211         td_sendbyte(0, TDBase);
 212         return 1;
 213     }
 214     return 0;
 215 }
 216
 217 UBYTE td_getprotstatus(UBYTE unitnum, struct TrackDiskBase *tdb)
 218 {
 219     /* Issue Sense Drive Status command to check whether write is possible */
 220     tdb->td_comsize = 2;
 221     tdb->td_rawcom[0] = FD_GETSTATUS;
 222     tdb->td_rawcom[1] = unitnum;
 223     td_sendcommand(tdb);
 224     td_readstatus(tdb,1);
 225     if (tdb->td_result[0] & 0x40)
 226         return(TDU_READONLY);
 227     else
 228         return(TDU_WRITABLE);
 229 }
 230
 231
 232 /*
 233  * Reset FDC, and initialize it again
 234  */
 235
 236 int td_dinit(struct TrackDiskBase *TDBase)
 237 {
 238     int i;
 239     UBYTE dor;
 240
 241     // Assert RESET in DOR
 242     dor = TDBase->td_dor & 0x39;
 243     outb(dor, FDC_DOR);
 244     outb(dor, FDC_DOR);
 245     // deassert RESET signal
 246     dor |= DORF_RESET;
 247     TDBase->td_dor = dor;
 248     outb(dor, FDC_DOR);
 249     td_waitint(TDBase,0,FALSE);
 250     /* Issue configure */
 251     td_configure(TDBase);
 252     // programm data rate
 253     outb(0,FDC_CCR);
 254     // issue Sense Interrupt Status (loop 4 times)
 255     for (i=0; i<4; i++)
 256     {
 257         TDBase->td_comsize = 1;
 258         TDBase->td_rawcom[0] = FD_SENSEI;
 259         td_sendcommand(TDBase);
 260         td_readstatus(TDBase, 2);
 261     }
 262     // issue Specify
 263     TDBase->td_comsize2 = 3;
 264     TDBase->td_rawcom2[0] = FD_SPECIFY;
 265     TDBase->td_rawcom2[1] = DP_SPEC1;
 266     TDBase->td_rawcom2[2] = DP_SPEC2;
 267     td_sendcommand2(TDBase);
 268     return 0;
 269 }
 270
 271 // Recalibrate (type != 0) or seek (type = 0) command.
 272 int td_recalibrate(unsigned char unitn, char type, int sector, struct TrackDiskBase *TDBase)
 273 {
 274     int err;
 275
 276     /* If we are not running already, this will spin up. */
 277     td_motoron(unitn,TDBase,FALSE);
 278
 279     if (type)
 280     {
 281         /* Issue specify */
 282         TDBase->td_comsize = 3;
 283         TDBase->td_rawcom[0] = FD_SPECIFY;
 284         TDBase->td_rawcom[1] = DP_SPEC1;
 285         TDBase->td_rawcom[2] = DP_SPEC2;
 286         td_sendcommand(TDBase);
 287         /* Issue recalibrate */
 288         TDBase->td_comsize = 2;
 289         TDBase->td_rawcom[0] = FD_RECALIBRATE;
 290         TDBase->td_rawcom[1] = unitn;
 291         err = td_sendcommand(TDBase);
 292         sector = 0;
 293     }
 294     else
 295     {
 296         /* Issue seek command */
 297         TDBase->td_comsize = 3;
 298         TDBase->td_rawcom[0] = FD_SEEK;
 299         TDBase->td_rawcom[1] = unitn;   /* Unit number */
 300         TDBase->td_rawcom[2] = sector;  /* New Track Number */
 301         err = td_sendcommand(TDBase);
 302     }
 303     if (!err)
 304     {
 305         /* Wait for interrupt */
 306         err = td_waitint(TDBase,0,TRUE);
 307         if (!err)
 308         {
 309             /* if drive doesn't report any error return 0 */
 310             if (((TDBase->td_sr0 & 0xf0) == 0x20) && (TDBase->td_pcn == sector))
 311             {
 312                 /* Store current track */
 313                 if (TDBase->td_Units[unitn])
 314                     TDBase->td_Units[unitn]->pub.tdu_CurrTrk=TDBase->td_pcn;
 315                 return 0;
 316             }
 317             /* SeekError otherwise */
 318             return TDERR_SeekError;
 319         }
 320     }
 321     return err;
 322 }
 323
 324 int td_rseek(UBYTE unitn, UBYTE dir, UBYTE cyls, struct TrackDiskBase *TDBase)
 325 {
 326     int err;
 327
 328     /* Select the unit, start motor */
 329     td_motoron(unitn,TDBase,FALSE);
 330
 331     if (dir)
 332     {
 333         /* Issue seek command */
 334         TDBase->td_comsize = 3;
 335         TDBase->td_rawcom[0] = FD_RSEEK_OUT;
 336         TDBase->td_rawcom[1] = unitn;   /* Unit number */
 337         TDBase->td_rawcom[2] = cyls;    /* New Track Number */
 338         err = td_sendcommand(TDBase);
 339     }
 340     else
 341     {
 342         TDBase->td_comsize = 3;
 343         TDBase->td_rawcom[0] = FD_RSEEK_IN;
 344         TDBase->td_rawcom[1] = unitn;
 345         TDBase->td_rawcom[2] = cyls;
 346         err = td_sendcommand(TDBase);
 347     }
 348     if (!err)
 349     {
 350         /* Wait for interrupt */
 351         err = td_waitint(TDBase,0,TRUE);
 352         if (!err)
 353         {
 354             /* if drive doesn't report any error return 0 */
 355             if (((TDBase->td_sr0 & 0xf0) == 0x20))
 356             {
 357                 if (TDBase->td_Units[unitn])
 358                     TDBase->td_Units[unitn]->pub.tdu_CurrTrk = TDBase->td_pcn;
 359                 return 0;
 360             }
 361             /* SeekError otherwise */
 362             return TDERR_SeekError;
 363         }
 364     }
 365     return err;
 366 }
 367
 368 /************************************************
 369  Name  : td_checkDisk
 370  Descr.: checks for disk in drive
 371  Input : none
 372  Output: 0 - disk in drive
 373          1 - no disk in drive
 374  Note  : The correct drive must be selected
 375 *************************************************/
 376 UBYTE td_getDiskChange( void )
 377 {
 378     return(inb(FDC_DIR)>>DIRB_DCHG);
 379 }
 380
 381
 382 int td_readwritetrack(UBYTE unitnum, char cyl, char hd, char mode, struct TrackDiskBase *TDBase)
 383 {
 384     int rwcnt;          // Read/Write retries
 385     int skcnt;          // Seek retries
 386     char *buf;
 387     int err;            // Error
 388
 389     skcnt = TDBase->td_Units[unitnum]->pub.tdu_RetryCnt;
 390     /* Program data rate */
 391     /* Gentlemen, start your engines */
 392     TDBase->td_Units[unitnum]->tdu_MotorOn = 1;
 393     td_motoron(unitnum,TDBase,TRUE);
 394     outb(0, FDC_CCR);   // 500kbit/s only!
 395     do
 396     {
 397         rwcnt = TDBase->td_Units[unitnum]->pub.tdu_RetryCnt;
 398         err = 0;
 399         /* If we are on the correct track, dont seek */
 400         if (TDBase->td_Units[unitnum]->pub.tdu_CurrTrk != cyl)
 401                 err = td_recalibrate(unitnum,0,cyl,TDBase);
 402         if (!err)
 403         {
 404             do
 405             {
 406                 /* Clear err flag */
 407                 err = 0;
 408                 /* Set DMA up */
 409                 clear_dma_ff(TD_DMA);
 410                 // Should place some cache flush in future (when cache implemented)
 411                 set_dma_addr(TD_DMA, (ULONG)(IPTR)(TDBase->td_Units[unitnum]->td_DMABuffer));
 412                 set_dma_count(TD_DMA, DP_SECTORS*512);
 413                 set_dma_mode(TD_DMA, (mode == FD_READ) ? DMA_MODE_READ : DMA_MODE_WRITE);
 414                 enable_dma(TD_DMA);
 415                 /* Issue read/write command */
 416                 TDBase->td_comsize = 9;
 417                 buf = TDBase->td_rawcom;
 418                 *buf++ = mode;                  // Command
 419                 *buf++ = unitnum | (hd << 2);   // Drive Select
 420                 *buf++ = cyl;                   // Cylinder
 421                 *buf++ = hd;                    // Head
 422                 *buf++ = 1;                     // First sector
 423                 *buf++ = DP_SSIZE;              // Sector size
 424                 *buf++ = DP_SECTORS;            // Last sector
 425                 *buf++ = DP_GAP1;               // Gap length
 426                 *buf = -1;                      // DTL
 427                 /* Command prepared, now send it */
 428                 td_sendcommand(TDBase);
 429                 /* Wait for end phase */
 430                 err = td_waitint(TDBase,7,FALSE);
 431                 if (!err)
 432                 {
 433                     /* Check if everything went OK */
 434                     if (!(TDBase->td_result[0] & 0xc0))
 435                     {
 436                         return 0;
 437                     }
 438                 }
 439                 /* Something went wrong. Let's see what. */
 440                 /* if err != then timeout err. */
 441                 if (!err)
 442                 {
 443                     err = TDERR_NotSpecified;
 444                     if (TDBase->td_result[1] & 0x80)
 445                         err = TDERR_TooFewSecs;
 446                     else if (TDBase->td_result[1] & 0x20)
 447                     {
 448                         err = TDERR_BadHdrSum;
 449                         if (TDBase->td_result[2] & 0x20)
 450                             err = TDERR_BadSecSum;
 451                     }
 452                     else if (TDBase->td_result[1] & 0x04)
 453                         err = TDERR_TooFewSecs;
 454                 }
 455             } while (--rwcnt);
 456         }
 457         td_recalibrate(unitnum,1,0,TDBase);
 458     } while(--skcnt);
 459     return err;
 460 }
 461
 462 int td_update(struct TDU *unit, struct TrackDiskBase *TDBase)
 463 {
 464     int err;            // Error
 465
 466     if (unit->tdu_flags & TDUF_WRITE)
 467     {
 468         if (td_getDiskChange())
 469             return TDERR_DiskChanged;   /* No disk in drive */
 470         err=td_readwritetrack(unit->tdu_UnitNum, unit->tdu_lastcyl, unit->tdu_lasthd, FD_WRITE, TDBase);
 471         if (err)
 472             return err;
 473         unit->tdu_flags &= ~TDUF_WRITE;
 474     }
 475     return 0;
 476 }
 477
 478 int td_readtrack(struct IOExtTD *iotd, UBYTE cyl, UBYTE hd, struct TrackDiskBase *TDBase)
 479 {
 480     struct TDU *unit = (struct TDU *)iotd->iotd_Req.io_Unit;
 481     int err;            // Error
 482
 483     if (td_getDiskChange())
 484         return TDERR_DiskChanged;       /* No disk in drive */
 485     if ((unit->tdu_lastcyl!=cyl) || (unit->tdu_lasthd!=hd))
 486     {
 487         err=td_update(unit, TDBase);
 488         if (err)
 489         {
 490             return err;
 491         }
 492         err=td_readwritetrack(unit->tdu_UnitNum, cyl, hd, FD_READ, TDBase);
 493         if (err)
 494             return err;
 495         unit->tdu_lastcyl=cyl;
 496         unit->tdu_lasthd=hd;
 497     }
 498     return 0;
 499 }
 500
 501 int td_read(struct IOExtTD *iotd, struct TrackDiskBase *TDBase)
 502 {
 503     struct TDU *unit=(struct TDU *)iotd->iotd_Req.io_Unit;
 504     int cyl, hd, sec;   // C/H/S for operation
 505     ULONG length=0;
 506     ULONG size, remain;
 507     int err;            // Error
 508
 509     if (unit->tdu_DiskIn == TDU_NODISK)
 510     {
 511         iotd->iotd_Req.io_Actual = 0;
 512         return TDERR_DiskChanged;
 513     }
 514
 515     sec = iotd->iotd_Req.io_Offset >> 9; // sector is wrong right now (LBA)
 516     cyl = (sec >> 1) / DP_SECTORS; // cyl contains real cyl number
 517     sec %= 2*DP_SECTORS;        // sector on track (on both sides)
 518     hd = sec / DP_SECTORS;      // head number
 519     sec %= DP_SECTORS;          // real sector number
 520     remain = iotd->iotd_Req.io_Length;
 521     while (length<iotd->iotd_Req.io_Length)
 522     {
 523         size = (DP_SECTORS*512) - (sec*512);
 524         if (size>remain)
 525             size=remain;
 526         err=td_readtrack(iotd, (UBYTE)cyl, (UBYTE)hd, TDBase);
 527         if (err)
 528         {
 529             iotd->iotd_Req.io_Actual = length;
 530             return err;
 531         }
 532         CopyMem(unit->td_DMABuffer+(sec*512), iotd->iotd_Req.io_Data+length, size);
 533         length += size;
 534         remain -= size;
 535         sec = 0;
 536         if (hd)
 537         {
 538             hd=0;
 539             cyl++;
 540         }
 541         else
 542             hd=1;
 543     }
 544     iotd->iotd_Req.io_Actual = iotd->iotd_Req.io_Length;
 545     return 0;
 546 }
 547
 548 int td_write(struct IOExtTD *iotd, struct TrackDiskBase *TDBase)
 549 {
 550     struct TDU *unit=(struct TDU *)iotd->iotd_Req.io_Unit;
 551     int cyl, hd, sec;   // C/H/S for operation
 552     ULONG length=0;
 553     ULONG size, remain;
 554     int err;            // Error
 555
 556     if (unit->tdu_DiskIn == TDU_NODISK)
 557     {
 558         iotd->iotd_Req.io_Actual = 0;
 559         return TDERR_DiskChanged;
 560     }
 561
 562     sec = iotd->iotd_Req.io_Offset >> 9; // sector is wrong right now (LBA)
 563     cyl = (sec >> 1) / DP_SECTORS;       // cyl contains real cyl number
 564     sec %= 2*DP_SECTORS;                 // sector on track (on both sides)
 565     hd = sec / DP_SECTORS;               // head number
 566     sec %= DP_SECTORS;                   // real sector number
 567
 568     remain = iotd->iotd_Req.io_Length;
 569     while (length<iotd->iotd_Req.io_Length)
 570     {
 571         size = (DP_SECTORS*512) - (sec*512);
 572         if (size>remain)
 573             size=remain;
 574         if (size<(DP_SECTORS*512))
 575         {
 576             /* read new track only if we don't
 577                want to write a full track
 578                */
 579             err=td_readtrack(iotd, (UBYTE)cyl, (UBYTE)hd, TDBase);
 580             if (err) {
 581                 iotd->iotd_Req.io_Actual = length;
 582                 return err;
 583             }
 584         }
 585         else
 586         {
 587             err=td_update(unit, TDBase);
 588             if (err)
 589                 return err;
 590             unit->tdu_lastcyl=(UBYTE)cyl;
 591             unit->tdu_lasthd=(UBYTE)hd;
 592         }
 593         CopyMem(iotd->iotd_Req.io_Data+length, unit->td_DMABuffer+(sec*512), size);
 594         unit->tdu_flags |= TDUF_WRITE;
 595         length += size;
 596         remain -= size;
 597         sec = 0;
 598         if (hd)
 599         {
 600             hd=0;
 601             cyl++;
 602         }
 603         else
 604             hd=1;
 605     }
 606     iotd->iotd_Req.io_Actual = iotd->iotd_Req.io_Length;
 607     return 0;
 608 }
 609
 610 /*
 611  * Format a track
 612  * Probably needs a lot more error checking/handling,
 613  * but at least it works.
 614  */
 615 int td_formattrack(struct TDU *unit, UBYTE cyl, UBYTE hd, struct TrackDiskBase *tdb)
 616 {
 617     UBYTE *dmabuf;
 618     int skcnt, rwcnt;
 619     int i,off;
 620     int err;            // Error
 621
 622     skcnt = unit->pub.tdu_RetryCnt;
 623     /* Start motor */
 624     unit->tdu_MotorOn = 1;
 625     td_motoron(unit->tdu_UnitNum,tdb,TRUE);
 626     /* Set datarate */
 627     outb(0,FDC_CCR);
 628
 629     /* We start by filling the DMA buffer with values needed */
 630     dmabuf = (UBYTE *)unit->td_DMABuffer;
 631     off=0;
 632     for (i=1;i<=DP_SECTORS;i++)
 633     {
 634         dmabuf[off++] = (UBYTE)cyl;
 635         dmabuf[off++] = hd;
 636         dmabuf[off++] = i;
 637         dmabuf[off++] = 2;
 638     }
 639     unit->tdu_lastcyl = -1;
 640     unit->tdu_lasthd = -1;
 641     do
 642     {
 643         /* Go to the correct track,. If we are already on it, we omit seeking in order to
 644            speed up the operation. */
 645         if (unit->pub.tdu_CurrTrk == cyl)
 646             err = 0;
 647         else
 648             err = td_recalibrate(unit->tdu_UnitNum,0,cyl,tdb);
 649         if (!err)
 650         {
 651             rwcnt = unit->pub.tdu_RetryCnt;
 652             do
 653             {
 654                 /* Set DMA up */
 655                 clear_dma_ff(TD_DMA);
 656                 // Should place some cache flush in future (when cache implemented)
 657                 set_dma_addr(TD_DMA, (ULONG)(IPTR)unit->td_DMABuffer);
 658                 set_dma_count(TD_DMA, DP_SECTORS*512);
 659                 set_dma_mode(TD_DMA, DMA_MODE_WRITE);
 660                 enable_dma(TD_DMA);
 661                 /* Issue format command */
 662                 tdb->td_comsize = 6;
 663                 tdb->td_rawcom[0] = FD_FORMAT;
 664                 tdb->td_rawcom[1] = unit->tdu_UnitNum | (hd << 2);
 665                 tdb->td_rawcom[2] = DP_SSIZE;
 666                 tdb->td_rawcom[3] = DP_SECTORS;
 667                 tdb->td_rawcom[4] = DP_GAP2;
 668                 tdb->td_rawcom[5] = 0;
 669                 /* Command prepared, now send it */
 670                 td_sendcommand(tdb);
 671                 /* Wait for end phase */
 672                 err = td_waitint(tdb,7,FALSE);
 673                 if (!err)
 674                 {
 675                     /* Check if everything went OK */
 676                     if (tdb->td_result[0] & 0xc0)
 677                     {
 678                         err = TDERR_NotSpecified;
 679                     }
 680                 }
 681                 if (!err)
 682                     return 0;
 683             } while (--rwcnt);
 684         }
 685         td_recalibrate(unit->tdu_UnitNum,1,0,tdb);
 686     } while(--skcnt);
 687     return err;
 688 }
 689
 690 int td_format(struct IOExtTD *iotd, struct TrackDiskBase *tdb)
 691 {
 692 #ifdef NOFORMAT
 693     iotd->iotd_Req.io_Actual = 0;
 694     return TDERR_WriteProt;
 695 #else
 696     struct TDU *unit=(struct TDU *)iotd->iotd_Req.io_Unit;
 697     ULONG size, remain;
 698     ULONG length = 0;
 699     int cyl, hd, sec;
 700     int err;            // Error
 701
 702     if (unit->tdu_DiskIn == TDU_NODISK)
 703     {
 704         D(bug("td_format(): No disk in drive!\n"));
 705         return TDERR_DiskChanged;
 706     }
 707
 708     /* Calculate CHS style address */
 709     sec = iotd->iotd_Req.io_Offset >> 9; // sector is wrong right now (LBA)
 710     cyl = (sec >> 1) / DP_SECTORS;       // cyl contains real cyl number
 711     sec %= 2*DP_SECTORS;                 // sector on track (on both sides)
 712     hd = sec / DP_SECTORS;               // head number
 713
 714     remain = iotd->iotd_Req.io_Length;
 715     while (length < iotd->iotd_Req.io_Length) {
 716         err = td_formattrack(unit, cyl, hd, tdb);
 717         if (!err)
 718         {
 719             /* We are fine, now write that data! */
 720             size = remain;
 721             if (size > 512*DP_SECTORS)
 722                 size = 512*DP_SECTORS;
 723             CopyMem(iotd->iotd_Req.io_Data + length, unit->td_DMABuffer, size);
 724             /* Note that we don't remember last track number stored in the buffer.
 725                We do it because next TD_READ issued in order to verify the data
 726                should really read the data from the disk, not just get them back
 727                from the buffer. */
 728             err = td_readwritetrack(unit->tdu_UnitNum, cyl, hd, FD_WRITE, tdb);
 729         }
 730         if (err) {
 731             iotd->iotd_Req.io_Actual = length;
 732             return err;
 733         }
 734         length += size;
 735         remain -= size;
 736         if (hd)
 737         {
 738             hd=0;
 739             cyl++;
 740         }
 741         else
 742             hd=1;
 743     }
 744     iotd->iotd_Req.io_Actual = iotd->iotd_Req.io_Length;
 745     return 0;
 746 #endif
 747 }
 748