arch/m68k-amiga/devs/trackdisk/trackdisk_hw.c

   1 /*
   2     Copyright © 1995-2016, The AROS Development Team. All rights reserved.
   3     $Id$
   4
   5     Desc: Amiga HW floppy stuff
   6     Lang: English
   7 */
   8
   9 #include <exec/types.h>
  10 #include <devices/trackdisk.h>
  11 #include <devices/timer.h>
  12 #include <hardware/cia.h>
  13 #include <hardware/custom.h>
  14 #include <proto/cia.h>
  15 #include <proto/disk.h>
  16 #include <proto/exec.h>
  17
  18 #include <resources/disk.h>
  19
  20 #include "trackdisk_device.h"
  21 #include "trackdisk_hw.h"
  22
  23 #define DEBUG 0
  24 #include <aros/debug.h>
  25
  26 #define QUICKRETRYRCNT 8 // re-read retries before reseeking, must be power of 2
  27
  28 #define ioStd(x)  ((struct IOStdReq *)x)
  29
  30 static void td_wait_start(struct TrackDiskBase *tdb, UWORD millis)
  31 {
  32     // do not remove, AbortIO()+WaitIO() does not clear signal bit
  33     // if AbortIO() finishes before WaitIO() waits.
  34     SetSignal(0, 1L << tdb->td_TimerMP2->mp_SigBit);
  35     tdb->td_TimerIO2->tr_node.io_Command = TR_ADDREQUEST;
  36     tdb->td_TimerIO2->tr_time.tv_secs = millis / 1000;
  37     tdb->td_TimerIO2->tr_time.tv_micro = (millis % 1000) * 1000;
  38     SendIO((struct IORequest *)tdb->td_TimerIO2);
  39 }
  40 static void td_wait_end(struct TrackDiskBase *tdb)
  41 {
  42     WaitIO((struct IORequest*)tdb->td_TimerIO2);
  43 }
  44 static void td_wait(struct TrackDiskBase *tdb, UWORD millis)
  45 {
  46     td_wait_start(tdb, millis);
  47     td_wait_end(tdb);
  48 }
  49
  50 static UBYTE const drvmask[] = { ~0x08, ~0x10, ~0x20, ~0x40 };
  51 void td_select(struct TDU *tdu, struct TrackDiskBase *tdb)
  52 {
  53     UBYTE tmp;
  54
  55     if (tdu->tdu_selected)
  56         return;
  57     tdu->tdu_selected = TRUE;
  58     tmp = tdb->ciab->ciaprb;
  59     tmp |= 0x08 | 0x10 | 0x20 | 0x40;
  60     tdb->ciab->ciaprb = tmp;
  61     if (tdu->tdu_MotorOn)
  62        tmp &= ~0x80;
  63     else
  64        tmp |= 0x80;
  65     tdb->ciab->ciaprb = tmp;
  66     tmp &= drvmask[tdu->tdu_UnitNum];
  67     tdb->ciab->ciaprb = tmp;
  68 }
  69
  70 void td_deselect(struct TDU *tdu, struct TrackDiskBase *tdb)
  71 {
  72     UBYTE tmp;
  73     if (!tdu->tdu_selected)
  74         return;
  75     tdu->tdu_selected = FALSE;
  76     tmp = tdb->ciab->ciaprb;
  77     tmp |= 0x08 | 0x10 | 0x20 | 0x40;
  78     tdb->ciab->ciaprb = tmp;
  79     tmp |= 0x80;
  80     tdb->ciab->ciaprb = tmp;
  81 }
  82
  83 static void td_setside(UBYTE side, struct TDU *tdu, struct TrackDiskBase *tdb)
  84 {
  85     if (!side) {
  86         tdb->ciab->ciaprb |= 0x4;
  87         tdu->pub.tdu_CurrTrk |= 1;
  88     } else {
  89         tdb->ciab->ciaprb &= ~0x04;
  90         tdu->pub.tdu_CurrTrk &= ~1;
  91     }
  92 }
  93
  94 static void td_setdirection(UBYTE dir, struct TDU *tdu, struct TrackDiskBase *tdb)
  95 {
  96     if (dir)
  97         tdb->ciab->ciaprb |= 0x02;
  98     else
  99         tdb->ciab->ciaprb &= ~0x02;
 100     if (dir != tdb->td_lastdir) {
 101         td_wait(tdb, 18);
 102         tdb->td_lastdir = dir;
 103     }
 104 }
 105
 106 static void td_step(struct TDU *tdu, struct TrackDiskBase *tdb, UBYTE delay)
 107 {
 108     tdb->ciab->ciaprb &= ~0x01;
 109     tdb->ciab->ciaprb |= 0x01;
 110     td_wait(tdb, delay);
 111 }
 112
 113 /* start motor */
 114 void td_motoron(struct TDU *tdu, struct TrackDiskBase *tdb, BOOL wait)
 115 {
 116     if (tdu->tdu_MotorOn)
 117         return;
 118     tdu->tdu_MotorOn = 1;
 119
 120     td_deselect(tdu, tdb);
 121     td_select(tdu, tdb);
 122     if (wait)
 123         td_wait(tdb, 500);
 124 }
 125
 126 /* stop motor */
 127 void td_motoroff(struct TDU *tdu, struct TrackDiskBase *tdb)
 128 {
 129     if (!tdu->tdu_MotorOn)
 130         return;
 131     tdu->tdu_MotorOn = 0;
 132
 133     td_deselect(tdu, tdb);
 134     td_select(tdu, tdb);
 135 }
 136
 137 static BOOL td_istrackzero(struct TDU *tdu, struct TrackDiskBase *tdb)
 138 {
 139     return (tdb->ciaa->ciapra & 0x10) == 0;
 140 }
 141
 142 UBYTE td_getprotstatus(struct TDU *tdu, struct TrackDiskBase *tdb)
 143 {
 144     UBYTE v;
 145     v = (tdb->ciaa->ciapra & 0x08) ? 0 : 1;
 146     return v;
 147 }
 148
 149 BOOL td_recalibrate(struct TDU *tdu, struct TrackDiskBase *tdb)
 150 {
 151     BYTE steps = 80 + 15;
 152     td_select(tdu, tdb);
 153     td_setside(0, tdu, tdb);
 154     td_wait(tdb, tdu->pub.tdu_CalibrateDelay);
 155     if (td_istrackzero(tdu, tdb)) {
 156         // step to cyl 1 if current cyl == 0
 157         td_setdirection(0, tdu, tdb);
 158         td_wait(tdb, tdu->pub.tdu_SettleDelay);
 159         td_step(tdu, tdb, tdu->pub.tdu_CalibrateDelay);
 160     }
 161     td_wait(tdb, tdu->pub.tdu_SettleDelay);
 162     td_setdirection(1, tdu, tdb);
 163     td_wait(tdb, tdu->pub.tdu_SettleDelay);
 164     while (!td_istrackzero(tdu, tdb)) {
 165         if (steps < 0) // drive is broken?
 166             return FALSE;
 167         td_step(tdu, tdb, tdu->pub.tdu_CalibrateDelay);
 168         steps--;
 169     }
 170     td_wait(tdb, tdu->pub.tdu_SettleDelay);
 171     tdu->pub.tdu_CurrTrk = 0;
 172     return TRUE;
 173 }
 174
 175 static UBYTE td_seek2(struct TDU *tdu, UBYTE cyl, UBYTE side, struct TrackDiskBase *tdb, BOOL nowait)
 176 {
 177     UBYTE dir;
 178     D(bug("seek=%d/%d\n", cyl, side));
 179     td_setside(side, tdu, tdb);
 180     if (tdu->pub.tdu_CurrTrk / 2 == cyl)
 181         return 0;
 182     if (tdu->pub.tdu_CurrTrk / 2 > cyl || cyl == 0xff)
 183         dir = 1;
 184     else
 185         dir = 0;
 186     td_setdirection(dir, tdu, tdb);
 187     while (cyl != tdu->pub.tdu_CurrTrk / 2) {
 188         td_step(tdu, tdb, tdu->pub.tdu_StepDelay);
 189         if (tdu->pub.tdu_CurrTrk / 2 > cyl && tdu->pub.tdu_CurrTrk >= 2)
 190             tdu->pub.tdu_CurrTrk -= 2;
 191         else if (tdu->pub.tdu_CurrTrk / 2 < cyl)
 192             tdu->pub.tdu_CurrTrk += 2;
 193         if (cyl == 0xff)
 194             break;
 195     }
 196     td_wait_start(tdb, tdu->pub.tdu_SettleDelay);
 197     if (!nowait)
 198         td_wait_end(tdb);
 199     return 0;
 200 }
 201 UBYTE td_seek(struct TDU *tdu, int cyl, int side, struct TrackDiskBase *tdb)
 202 {
 203     return td_seek2(tdu, cyl, side, tdb, FALSE);
 204 }
 205 UBYTE td_seek_nowait(struct TDU *tdu, int cyl, int side, struct TrackDiskBase *tdb)
 206 {
 207     return td_seek2(tdu, cyl, side, tdb, TRUE);
 208 }
 209
 210 // 0 = no disk, 1 = disk inserted
 211 UBYTE td_getDiskChange(struct TDU *tdu, struct TrackDiskBase *tdb)
 212 {
 213     UBYTE v;
 214     v = (tdb->ciaa->ciapra & 0x04) ? 1 : 0;
 215     return v;
 216 }
 217
 218 static BOOL checkbuffer(struct TDU *tdu, struct TrackDiskBase *tdb)
 219 {
 220     // allocate HD sized buffer if HD disk inserted
 221     if ((tdu->tdu_hddisk && !tdb->td_supportHD) || !tdb->td_DMABuffer) {
 222         FreeMem(tdb->td_DMABuffer, DISK_BUFFERSIZE);
 223         FreeMem(tdb->td_DataBuffer, 11 * 512);
 224         tdb->td_DMABuffer = AllocMem(DISK_BUFFERSIZE * 2, MEMF_CHIP);
 225         tdb->td_DataBuffer = AllocMem(22 * 512, MEMF_ANY);
 226         if (!tdb->td_DMABuffer || !tdb->td_DataBuffer) {
 227             FreeMem(tdb->td_DMABuffer, DISK_BUFFERSIZE * 2);
 228             FreeMem(tdb->td_DataBuffer, 22 * 512);
 229             return 1;
 230         }
 231         tdb->td_supportHD = TRUE;
 232     }
 233     return 0;
 234 }
 235
 236 static UBYTE td_readwritetrack(UBYTE track, UBYTE write, struct TDU *tdu, struct TrackDiskBase *tdb)
 237 {
 238     UBYTE err = 0;
 239     ULONG sigs;
 240     UWORD dsklen = 0x8000 | ((DISK_BUFFERSIZE / 2) * (tdu->tdu_hddisk ? 2 : 1)) | (write ? 0x4000 : 0);
 241
 242     td_motoron(tdu, tdb, TRUE);
 243
 244     SetSignal(0, 1L << tdb->td_IntBit);
 245
 246     tdb->custom->adkcon = 0x0600;
 247     tdb->custom->adkcon = write ? 0x9100 : 0x9500;
 248     tdb->custom->intreq = 0x0002; // clear disk interrupt request
 249     tdb->custom->intena = 0x8002; // enable disk interrupt
 250     tdb->custom->dmacon = 0x8010; // enable DMA
 251
 252     tdb->custom->dskpt = tdb->td_DMABuffer;
 253     tdb->custom->dsklen = dsklen;
 254     tdb->custom->dsklen = dsklen; // dma started
 255     D(bug("td diskdma started, track=%d write=%d len=%d\n", track, write, dsklen & 0x3fff));
 256
 257     td_wait_start(tdb, (tdu->tdu_hddisk ? 2 : 1) * 1000);
 258     sigs = Wait((1L << tdb->td_TimerMP2->mp_SigBit) | (1L << tdb->td_IntBit));
 259
 260     tdb->custom->dsklen = 0x4000;
 261     tdb->custom->intena = 0x0002;
 262
 263     err = TDERR_BadSecPreamble;
 264     if (sigs & (1L << tdb->td_IntBit)) {
 265         // dma finished
 266         err = 0;
 267         AbortIO((struct IORequest *)tdb->td_TimerIO2);
 268     }
 269     WaitIO((struct IORequest *)tdb->td_TimerIO2);
 270     D(bug("td diskdma finished, err=%d\n", err));
 271
 272     if (td_getDiskChange(tdu, tdb) == 0)
 273         err = TDERR_DiskChanged;
 274
 275     return err;
 276 }
 277
 278 static void make_crc_table16(struct TrackDiskBase *tdb)
 279 {
 280         unsigned short w;
 281         int n, k;
 282
 283         tdb->crc_table16 = AllocMem(256 * sizeof(UWORD), MEMF_ANY);
 284         if (!tdb->crc_table16)
 285                 return;
 286         for (n = 0; n < 256; n++) {
 287                 w = n << 8;
 288                 for (k = 0; k < 8; k++) {
 289                         w = (w << 1) ^ ((w & 0x8000) ? 0x1021 : 0);
 290                 }
 291                 tdb->crc_table16[n] = w;
 292         }
 293 }
 294
 295 static UWORD get_crc16_next(struct TrackDiskBase *tdb, void *vbuf, UWORD len, UWORD crc)
 296 {
 297         UBYTE *buf = (UBYTE*)vbuf;
 298         while (len-- > 0)
 299                 crc = (crc << 8) ^ tdb->crc_table16[((crc >> 8) ^ (*buf++)) & 0xff];
 300         return crc;
 301 }
 302 static UWORD get_crc16(struct TrackDiskBase *tdb, void *vbuf, UWORD len)
 303 {
 304         if (!tdb->crc_table16) {
 305                 make_crc_table16(tdb);
 306                 if (!tdb->crc_table16)
 307                         return 0;
 308         }
 309         return get_crc16_next(tdb, vbuf, len, 0xffff);
 310 }
 311
 312 /* Following really needs optimization... */
 313 static UBYTE mfmdecode (UWORD **mfmp)
 314 {
 315         UWORD mfm;
 316         UBYTE out;
 317         UBYTE i;
 318
 319         mfm = **mfmp;
 320         out = 0;
 321         (*mfmp)++;
 322         for (i = 0; i < 8; i++) {
 323                 out >>= 1;
 324                 if (mfm & 1)
 325                         out |= 0x80;
 326                 mfm >>= 2;
 327         }
 328         return out;
 329 }
 330
 331 /* PC floppy format decoding is very expensive compared to ADOS... */
 332 static UBYTE td_decodebuffer_pcdos(struct TDU *tdu, struct TrackDiskBase *tdb)
 333 {
 334         UWORD *raw, *rawend;
 335         UWORD i;
 336         UBYTE lasterr;
 337         UBYTE *data = tdb->td_DataBuffer;
 338         BYTE sector;
 339         UBYTE tmp[8];
 340         UWORD datacrc;
 341         UBYTE current_head, current_cyl;
 342
 343         tmp[0] = tmp[1] = tmp[2] = 0xa1;
 344         tmp[3] = 0xfb;
 345         datacrc = get_crc16(tdb, tmp, 4);
 346         current_head = tdb->td_buffer_track & 1;
 347         current_cyl = tdb->td_buffer_track / 2;
 348
 349         lasterr = 0;
 350         raw = tdb->td_DMABuffer;
 351         rawend = tdb->td_DMABuffer + DISK_BUFFERSIZE * (tdu->tdu_hddisk ? 2 : 1);
 352         sector = -1;
 353         while (tdb->td_sectorbits != (1 << tdu->tdu_sectors) - 1) {
 354                 UBYTE *secdata;
 355                 UWORD crc;
 356                 UBYTE mark;
 357
 358                 if (raw != tdb->td_DMABuffer) {
 359                         while (*raw != 0x4489) {
 360                                 if (raw >= rawend) {
 361                                         if (lasterr == 0)
 362                                                 lasterr = TDERR_TooFewSecs;
 363                                         goto end;
 364                                 }
 365                                 raw++;
 366                         }
 367                 }
 368                 while (*raw == 0x4489 && raw < rawend)
 369                         raw++;
 370                 if (raw >= rawend - 8) {
 371                         if (lasterr == 0)
 372                                 lasterr = TDERR_TooFewSecs;
 373                         goto end;
 374                 }
 375                 mark = mfmdecode(&raw);
 376                 if (mark == 0xfe) {
 377                         UBYTE cyl, head, size;
 378
 379                         cyl = mfmdecode (&raw);
 380                         head = mfmdecode (&raw);
 381                         sector = mfmdecode (&raw);
 382                         size = mfmdecode (&raw);
 383                         crc = (mfmdecode (&raw) << 8) | mfmdecode (&raw);
 384
 385                         tmp[0] = 0xa1; tmp[1] = 0xa1; tmp[2] = 0xa1; tmp[3] = mark;
 386                         tmp[4] = cyl; tmp[5] = head; tmp[6] = sector; tmp[7] = size;
 387                         if (get_crc16(tdb, tmp, 8) != crc || cyl != current_cyl || head != current_head || size != 2 || sector < 1 || sector > tdu->tdu_sectors) {
 388                                 D(bug("PCDOS: corrupted sector header. cyl=%d head=%d sector=%d size=%d crc=%04x\n",
 389                                         cyl, head, sector, size, crc));
 390                                 sector = -1;
 391                                 continue;
 392                         }
 393                         sector--;
 394                         continue;
 395                 }
 396                 if (mark != 0xfb) {
 397                         D(bug("PCDOS: unknown address mark %02X\n", mark));
 398                         continue;
 399                 }
 400                 if (sector < 0)
 401                         continue;
 402                 if (raw >= rawend - 512) {
 403                         if (lasterr == 0)
 404                                 lasterr = TDERR_TooFewSecs;
 405                         goto end;
 406                 }
 407                 secdata = data + sector * 512;
 408                 for (i = 0; i < 512; i++)
 409                         secdata[i] = mfmdecode (&raw);
 410                 crc = (mfmdecode (&raw) << 8) | mfmdecode (&raw);
 411                 if (get_crc16_next(tdb, secdata, 512, datacrc) != crc) {
 412                         D(bug("PCDOS: sector %d data checksum error\n", sector + 1));
 413                         continue;
 414                 }
 415                 D(bug("PCDOS: read sector %d\n", sector));
 416                 tdb->td_sectorbits |= 1 << sector;
 417                 sector = -1;
 418         }
 419 end:
 420         D(bug("PCDOS: err=%d secmask=%08x\n", lasterr, tdb->td_sectorbits));
 421         return lasterr;
 422 }
 423
 424
 425 static ULONG getmfmlong(UWORD *mfmbuf)
 426 {
 427     return ((ULONG*)mfmbuf)[0] & 0x55555555;
 428 }
 429 static ULONG getdecodedlong(UWORD *mfmbuf, UWORD offset)
 430 {
 431     ULONG odd = getmfmlong(mfmbuf);
 432     ULONG even = getmfmlong(mfmbuf + offset);
 433     return (odd << 1) | even;
 434 }
 435 static ULONG getdecodedlongchk(UWORD *mfmbuf, UWORD offset, ULONG *chksum)
 436 {
 437     ULONG odd = getmfmlong(mfmbuf);
 438     ULONG even = getmfmlong(mfmbuf + offset);
 439     *chksum ^= odd ^ even;
 440     return (odd << 1) | even;
 441 }
 442
 443 static UBYTE td_decodebuffer_ados(struct TDU *tdu, struct TrackDiskBase *tdb)
 444 {
 445         UWORD *raw, *rawend;
 446         UBYTE i;
 447         UBYTE lasterr;
 448         UBYTE *data = tdb->td_DataBuffer;
 449
 450         lasterr = 0;
 451         raw = tdb->td_DMABuffer;
 452         rawend = tdb->td_DMABuffer + DISK_BUFFERSIZE * (tdu->tdu_hddisk ? 2 : 1);
 453         while (tdb->td_sectorbits != (1 << tdu->tdu_sectors) - 1) {
 454             UWORD *rawnext = raw;
 455             UBYTE *secdata;
 456             ULONG chksum, id, dlong;
 457             UBYTE trackoffs;
 458
 459             if (raw != tdb->td_DMABuffer) {
 460                 while (*raw != 0x4489) {
 461                     if (raw >= rawend) {
 462                         if (lasterr == 0)
 463                             lasterr = TDERR_TooFewSecs;
 464                         goto end;
 465                     }
 466                     raw++;
 467                 }
 468             }
 469             while (*raw == 0x4489 && raw < rawend)
 470                 raw++;
 471             if (raw + 544 >= rawend) {
 472                 if (lasterr == 0)
 473                     lasterr = TDERR_TooFewSecs;
 474                 goto end;
 475             }
 476
 477             rawnext = raw + 544 - 3;
 478
 479             chksum = 0;
 480             id = getdecodedlongchk(raw, 2, &chksum);
 481             raw += 4;
 482
 483             trackoffs = (id & 0xff00) >> 8;
 484             if (trackoffs >= tdu->tdu_sectors || (id & 0xff000000) != 0xff000000) {
 485                 lasterr = TDERR_BadSecHdr;
 486                 D(bug("td_decodebuffer trackid=%08x\n", id));
 487                 continue; // corrupt sector number
 488             }
 489             if (tdb->td_sectorbits & (1 << trackoffs)) {
 490                 // skip sector if it has already been successfully decoded and copied
 491                 raw = rawnext;
 492                 continue;
 493             }
 494             // decode header
 495             for (i = 0; i < 4; i++) {
 496                 getdecodedlongchk(raw, 8, &chksum);
 497                 raw += 2;
 498             }
 499             raw += 8;
 500             dlong = getdecodedlong(raw, 2);
 501             raw += 4;
 502             // header checksum ok?
 503             if (dlong != chksum) {
 504                 lasterr = TDERR_BadHdrSum;
 505                 D(bug("td_decodebuffer sector %d header checksum error\n", trackoffs));
 506                 continue;
 507             }
 508             // correct track?
 509             if (((id & 0x00ff0000) >> 16) != tdb->td_buffer_track) {
 510                 lasterr = TDERR_BadSecHdr;
 511                 D(bug("td_decodebuffer sector %d header track mismatch (%d<>%d)\n",
 512                         trackoffs, (id & 0x00ff0000) >> 16, tdb->td_buffer_track));
 513                 continue;
 514             }
 515
 516             // decode data
 517             chksum = getdecodedlong(raw, 2);
 518             raw += 4;
 519             secdata = data + trackoffs * 512;
 520             for (i = 0; i < 128; i++) {
 521                 dlong = getdecodedlongchk(raw, 256, &chksum);
 522                 raw += 2;
 523                 *secdata++ = dlong >> 24;
 524                 *secdata++ = dlong >> 16;
 525                 *secdata++ = dlong >> 8;
 526                 *secdata++ = dlong;
 527             }
 528             if (chksum) {
 529                 lasterr = TDERR_BadSecSum;
 530                 D(bug("td_decodebuffer sector %d data checksum error\n", trackoffs));
 531                 continue; // data checksum error
 532             }
 533             tdb->td_sectorbits |= 1 << trackoffs;
 534         }
 535 end:
 536         D(bug("td_decodebuffer err=%d secmask=%08x\n", lasterr, tdb->td_sectorbits));
 537         return lasterr;
 538 }
 539
 540 static UBYTE td_decodebuffer(struct TDU *tdu, struct TrackDiskBase *tdb)
 541 {
 542         if (tdu->tdu_disktype == DT_ADOS)
 543                 return td_decodebuffer_ados(tdu, tdb);
 544         else if (tdu->tdu_disktype == DT_PCDOS)
 545                 return td_decodebuffer_pcdos(tdu, tdb);
 546         return TDERR_TooFewSecs;
 547 }
 548
 549 static void mfmcode (ULONG *mfm, UWORD longs)
 550 {
 551     ULONG prev = mfm[-1];
 552         while (longs--) {
 553                 ULONG v = (*mfm) & 0x55555555;
 554                 // if mask1 is not set (previous bit) and mask2 (current bit) is not set: set bit.
 555                 ULONG mask1 = (prev << 31) | (v >> 1);
 556                 ULONG mask2 = v << 1;
 557         // mfmbits bit set only if bits in both masks are zero.
 558                 ULONG mfmbits = ((~mask1) & (~mask2)) & 0xaaaaaaaa;
 559                 prev = v;
 560                 v |= mfmbits;
 561                 *mfm++ = v;
 562         }
 563 }
 564
 565 static void td_encodebuffer_ados(struct TDU *tdu, struct TrackDiskBase *tdb)
 566 {
 567         UWORD i;
 568         UBYTE sec;
 569         UBYTE *databuf = tdb->td_DataBuffer;
 570         ULONG *mfmbuf = (ULONG*)tdb->td_DMABuffer;
 571         UWORD bufsize = DISK_BUFFERSIZE * (tdu->tdu_hddisk ? 2 : 1);
 572         UWORD gapsize = bufsize - tdu->tdu_sectors * 2 * 544;
 573
 574         for (i = 0; i < gapsize / 4 - 1; i++)
 575                 *mfmbuf++ = 0xaaaaaaaa;
 576
 577         for (sec = 0; sec < tdu->tdu_sectors; sec++) {
 578                 ULONG deven, dodd;
 579                 ULONG hck = 0, dck = 0;
 580
 581         // make sure we have valid MFM clock bit here!
 582         if (mfmbuf[-1] & 1)
 583             mfmbuf[0] = 0x2aaaaaaa;
 584         else
 585                     mfmbuf[0] = 0xaaaaaaaa;
 586                 mfmbuf[1] = 0x44894489;
 587
 588                 deven = (0xff << 24) | (tdb->td_buffer_track << 16) | (sec << 8) | ((tdu->tdu_sectors - sec) << 0);
 589                 dodd = deven >> 1;
 590                 dodd &= 0x55555555;
 591                 deven &= 0x55555555;
 592                 mfmbuf[2] = dodd;
 593                 mfmbuf[3] = deven;
 594                 hck ^= dodd;
 595                 hck ^= deven;
 596
 597                 for (i = 4; i < 12; i++)
 598                         mfmbuf[i] = 0;
 599
 600                 for (i = 0; i < 128; i++) {
 601                         deven = ((ULONG*)databuf)[i];
 602                         dodd = deven >> 1;
 603                         dodd &= 0x55555555;
 604                         deven &= 0x55555555;
 605                         mfmbuf[  0 + 16 + i] = dodd;
 606                         mfmbuf[128 + 16 + i] = deven;
 607                         dck ^= dodd;
 608                         dck ^= deven;
 609                 }
 610
 611                 deven = hck;
 612                 dodd = deven >> 1;
 613                 mfmbuf[12] = dodd;
 614                 mfmbuf[13] = deven;
 615
 616                 deven = dck;
 617                 dodd = deven >> 1;
 618                 mfmbuf[14] = dodd;
 619                 mfmbuf[15] = deven;
 620
 621                 mfmcode (mfmbuf + 2, 2 * 128 + 16 - 2);
 622
 623                 databuf += 512;
 624                 mfmbuf += 2 * 128 + 16;
 625         }
 626         mfmbuf[0] = 0;
 627         mfmcode (mfmbuf, 1);
 628 }
 629
 630 static void td_encodebuffer(struct TDU *tdu, struct TrackDiskBase *tdb)
 631 {
 632         if (tdu->tdu_disktype == DT_ADOS)
 633                 td_encodebuffer_ados(tdu, tdb);
 634 }
 635
 636 static UBYTE td_readbuffer(UBYTE track, struct TDU *tdu, struct TrackDiskBase *tdb)
 637 {
 638         UBYTE ret;
 639
 640         if (tdb->td_buffer_unit != tdu->tdu_UnitNum || tdb->td_buffer_track != track)
 641                 tdb->td_sectorbits = 0;
 642         tdb->td_buffer_unit = tdu->tdu_UnitNum;
 643         tdb->td_buffer_track = track;
 644         td_select(tdu, tdb);
 645         td_seek(tdu, track >> 1, track & 1, tdb);
 646         ret = td_readwritetrack(track, 0, tdu, tdb);
 647         if (ret) {
 648                 D(bug("td_readbuffer TRK=%d td_readwritetrack ERR=%d\n", track, ret));
 649                 tdb->td_sectorbits = 0;
 650                 return ret;
 651         }
 652         ret = td_decodebuffer(tdu, tdb);
 653         D(bug("td_readbuffer td_decodebuffer ERR=%d MASK=%08x\n", ret, tdb->td_sectorbits));
 654         return ret;
 655 }
 656
 657 static void maybe_detect(struct TDU *tdu, struct TrackDiskBase *tdb)
 658 {
 659         if (tdu->tdu_disktype == DT_UNDETECTED)
 660                 td_detectformat(tdu, tdb);
 661 }
 662
 663 static UBYTE maybe_flush(struct TDU *tdu, struct TrackDiskBase *tdb, int track)
 664 {
 665         if (tdb->td_buffer_unit != tdu->tdu_UnitNum || tdb->td_buffer_track != track) {
 666                 UBYTE err = 0;
 667                 err = td_flush(tdu, tdb);
 668                 td_clear(tdb);
 669                 return err;
 670         }
 671         return 0;
 672 }
 673
 674 UBYTE td_read(struct IOExtTD *iotd, struct TDU *tdu, struct TrackDiskBase *tdb)
 675 {
 676         UBYTE err;
 677         APTR data;
 678         ULONG len, offset;
 679         WORD totalretries;
 680         BYTE seeking;
 681
 682         if (tdu->tdu_DiskIn == TDU_NODISK)
 683                 return TDERR_DiskChanged;
 684         if (checkbuffer(tdu, tdb))
 685                 return TDERR_NoMem;
 686         maybe_detect(tdu, tdb);
 687
 688         iotd->iotd_Req.io_Actual = 0;
 689         offset = iotd->iotd_Req.io_Offset;
 690         len = iotd->iotd_Req.io_Length;
 691         data = iotd->iotd_Req.io_Data;
 692
 693         D(bug("td_read: DATA=%x OFFSET=%x (TRK=%d) LEN=%d\n", data, offset, offset / (512 * tdu->tdu_sectors), len));
 694
 695         seeking = 0;
 696         err = 0;
 697         totalretries = (tdu->pub.tdu_RetryCnt + 1) * QUICKRETRYRCNT - 1;
 698
 699         while (len > 0 && totalretries >= 0) {
 700
 701                 UBYTE largestsectorneeded, smallestsectorneeded, totalsectorsneeded;
 702                 UBYTE track;
 703                 UBYTE sec, sectorsdone;
 704
 705                 track = offset / (512 * tdu->tdu_sectors);
 706
 707                 if (seeking)
 708                         td_wait_end(tdb);
 709
 710                 if ((totalretries % QUICKRETRYRCNT) == 0) {
 711                         if (!td_recalibrate(tdu, tdb)) {
 712                                 err = TDERR_SeekError;
 713                                 break;
 714                         }
 715                 }
 716
 717                 err = maybe_flush(tdu, tdb, track);
 718                 if (err)
 719                         break;
 720                 if (tdb->td_buffer_unit != tdu->tdu_UnitNum || tdb->td_buffer_track != track)
 721                         err = td_readbuffer(track, tdu, tdb);
 722
 723                 smallestsectorneeded = (offset / 512) % tdu->tdu_sectors;
 724                 largestsectorneeded = smallestsectorneeded + len / 512;
 725                 if (largestsectorneeded > tdu->tdu_sectors || len / 512 > tdu->tdu_sectors) {
 726                         UBYTE nexttrack = track + 1;
 727                         if (nexttrack < 160) {
 728                                 // start stepping to next track in advance (pointless but..)
 729                                 td_seek_nowait(tdu, nexttrack >> 1, nexttrack & 1, tdb);
 730                                 seeking = 1;
 731                         }
 732                         largestsectorneeded = tdu->tdu_sectors;
 733                 }
 734                 totalsectorsneeded = largestsectorneeded - smallestsectorneeded;
 735
 736                 sectorsdone = 0;
 737                 for (sec = smallestsectorneeded; sec < largestsectorneeded; sec++) {
 738                         if (tdb->td_sectorbits & (1 << sec)) {
 739                                 CopyMem(tdb->td_DataBuffer + sec * 512, data + (sec - smallestsectorneeded) * 512, 512);
 740                                 sectorsdone++;
 741                         }
 742                 }
 743
 744                 D(bug("td_read2 TRK=%d MIN=%d MAX=%d DONE=%d\n", track, smallestsectorneeded, largestsectorneeded, sectorsdone));
 745
 746                 if (sectorsdone < totalsectorsneeded) {
 747                         // errors, force re-read
 748                         tdb->td_buffer_unit = -1;
 749                         // couldn't decode any sectors = reseek immediately
 750                         if (tdb->td_sectorbits == 0)
 751                                 totalretries = (totalretries - 1) & ~(QUICKRETRYRCNT - 1);
 752                         else
 753                                 totalretries--;
 754                         continue;
 755                 }
 756
 757                 data += sectorsdone * 512;
 758                 offset += sectorsdone * 512;
 759                 len -= sectorsdone * 512;
 760                 iotd->iotd_Req.io_Actual += sectorsdone * 512;
 761
 762                 err = 0;
 763         }
 764
 765         if (seeking)
 766                 td_wait_end(tdb);
 767         D(bug("td_read2 ERR=%d io_Actual=%d\n", err, iotd->iotd_Req.io_Actual));
 768         return err;
 769 }
 770
 771 static UBYTE td_write2(struct IOExtTD *iotd, struct TDU *tdu, struct TrackDiskBase *tdb)
 772 {
 773         APTR data;
 774         ULONG len, offset;
 775         UBYTE err;
 776
 777         if (checkbuffer(tdu, tdb))
 778                 return TDERR_NoMem;
 779
 780         err = 0;
 781         iotd->iotd_Req.io_Actual = 0;
 782         offset = iotd->iotd_Req.io_Offset;
 783         len = iotd->iotd_Req.io_Length;
 784         data = iotd->iotd_Req.io_Data;
 785
 786         D(bug("TD_WRITE: DATA=%x OFFSET=%x (TRK=%d) LEN=%d\n", data, offset, offset / (512 * tdu->tdu_sectors), len));
 787
 788         while (len > 0) {
 789                 UBYTE track, sec, totalsectorsneeded;
 790                 UBYTE smallestsectorneeded, largestsectorneeded;
 791
 792                 track = offset / (512 * tdu->tdu_sectors);
 793                 err = maybe_flush(tdu, tdb, track);
 794                 if (err)
 795                         break;
 796                 tdb->td_buffer_unit = tdu->tdu_UnitNum;
 797                 tdb->td_buffer_track = track;
 798
 799                 smallestsectorneeded = (offset / 512) % tdu->tdu_sectors;
 800                 largestsectorneeded = smallestsectorneeded + len / 512;
 801                 if (largestsectorneeded > tdu->tdu_sectors || len / 512 > tdu->tdu_sectors)
 802                         largestsectorneeded = tdu->tdu_sectors;
 803                 totalsectorsneeded = largestsectorneeded - smallestsectorneeded;
 804
 805                 D(bug("TD_WRITE: TRK=%d MIN=%d MAX=%d MASK=%08x\n",
 806                         track, smallestsectorneeded, largestsectorneeded, tdb->td_sectorbits));
 807
 808                 // fill buffer with new data
 809                 for (sec = smallestsectorneeded; sec < largestsectorneeded; sec++) {
 810                         CopyMem(data + (sec - smallestsectorneeded) * 512, tdb->td_DataBuffer + sec * 512, 512);
 811                         tdb->td_sectorbits |= 1 << sec;
 812                         tdb->td_dirty = TRUE;
 813                 }
 814
 815                 data += totalsectorsneeded * 512;
 816                 offset += totalsectorsneeded * 512;
 817                 len -= totalsectorsneeded * 512;
 818                 iotd->iotd_Req.io_Actual += totalsectorsneeded * 512;
 819         }
 820         return err;
 821 }
 822
 823 UBYTE td_write(struct IOExtTD *iotd, struct TDU *tdu, struct TrackDiskBase *tdb)
 824 {
 825     UBYTE err;
 826     if (tdu->tdu_DiskIn == TDU_NODISK)
 827         return TDERR_DiskChanged;
 828     if (tdu->tdu_disktype != DT_ADOS)
 829         return TDERR_WriteProt;
 830     if (!td_getprotstatus(tdu, tdb)) {
 831         err = td_write2(iotd, tdu, tdb);
 832     } else {
 833         err = TDERR_WriteProt;
 834     }
 835     return err;
 836 }
 837
 838 void td_clear(struct TrackDiskBase *tdb)
 839 {
 840         tdb->td_buffer_unit = -1;
 841         tdb->td_sectorbits = 0;
 842         tdb->td_dirty = 0;
 843 }
 844
 845 UBYTE td_flush(struct TDU *tdu, struct TrackDiskBase *tdb)
 846 {
 847         WORD totalretries;
 848         UBYTE lasterr, err;
 849
 850         if (tdb->td_buffer_unit != tdu->tdu_UnitNum)
 851                 return 0;
 852         if (!tdb->td_dirty)
 853                 return 0;
 854
 855         err = 0;
 856         td_select(tdu, tdb);
 857         td_seek(tdu, tdb->td_buffer_track >> 1, tdb->td_buffer_track & 1, tdb);
 858         D(bug("td_flush, writing unit %d track %d wmask=%08x\n",
 859                 tdb->td_buffer_unit, tdb->td_buffer_track, tdb->td_sectorbits));
 860
 861         totalretries = (tdu->pub.tdu_RetryCnt + 1) * QUICKRETRYRCNT - 1;
 862         // read all non-modified sectors from disk (if needed)
 863         lasterr = 0;
 864         while (tdb->td_sectorbits != (1 << tdu->tdu_sectors) - 1) {
 865                 if ((totalretries % QUICKRETRYRCNT) == 0) {
 866                         if (!td_recalibrate(tdu, tdb)) {
 867                                 tdb->td_dirty = 0;
 868                                 tdb->td_buffer_unit = -1;
 869                                 return TDERR_SeekError;
 870                         }
 871                 }
 872                 err = td_readbuffer(tdb->td_buffer_track, tdu, tdb);
 873                 if (err)
 874                         lasterr = err;
 875                 D(bug("TD_FLUSH READBUF ERR=%d MASK=%08x\n", err, tdb->td_sectorbits));
 876                 if (totalretries-- <= 0) {
 877                         bug("TD_FLUSH disk error track %d, written data was lost!\n", tdb->td_buffer_track);
 878                         tdb->td_dirty = 0;
 879                         tdb->td_buffer_unit = -1;
 880                         return lasterr ? lasterr : TDERR_NotSpecified;
 881                 }
 882         }
 883         // MFM encode buffer
 884         td_encodebuffer(tdu, tdb);
 885         // write buffer
 886         err = td_readwritetrack(tdb->td_buffer_track, 1, tdu, tdb);
 887         td_wait(tdb, 2);
 888         tdb->td_dirty = 0;
 889         return err;
 890 }
 891
 892 static UBYTE td_format2(struct IOExtTD *iotd, struct TDU *tdu, struct TrackDiskBase *tdb)
 893 {
 894     APTR data;
 895     ULONG len, offset;
 896     UBYTE err = 0;
 897
 898     if (checkbuffer(tdu, tdb))
 899         return TDERR_NoMem;
 900     if (tdu->tdu_disktype != DT_ADOS)
 901         return TDERR_WriteProt;
 902     iotd->iotd_Req.io_Actual = 0;
 903     offset = iotd->iotd_Req.io_Offset;
 904     len = iotd->iotd_Req.io_Length;
 905     data = iotd->iotd_Req.io_Data;
 906     D(bug("TD_FORMAT: DATA=%x OFFSET=%x (TRK=%d) LEN=%d\n", data, offset, offset / (512 * tdu->tdu_sectors), len));
 907     while (len >= tdu->tdu_sectors * 512) {
 908         int track = offset / (512 * tdu->tdu_sectors);
 909         td_select(tdu, tdb);
 910         td_wait(tdb, 2);
 911         td_seek(tdu, track >> 1, track & 1, tdb);
 912         CopyMem(data, tdb->td_DataBuffer, tdu->tdu_sectors * 512);
 913         tdb->td_sectorbits = (1 << tdu->tdu_sectors) - 1;
 914         tdb->td_buffer_unit = tdu->tdu_UnitNum;
 915         tdb->td_buffer_track = track;
 916         td_encodebuffer(tdu, tdb);
 917         err = td_readwritetrack(tdb->td_buffer_track, 1, tdu, tdb);
 918         td_clear(tdb);
 919         if (err)
 920             return err;
 921         data += tdu->tdu_sectors * 512;
 922         offset += tdu->tdu_sectors * 512;
 923         iotd->iotd_Req.io_Actual += tdu->tdu_sectors * 512;
 924         len -= tdu->tdu_sectors * 512;
 925     }
 926     td_wait(tdb, 2);
 927     return err;
 928 }
 929
 930 UBYTE td_format(struct IOExtTD *iotd, struct TDU *tdu, struct TrackDiskBase *tdb)
 931 {
 932     UBYTE err;
 933     if (tdu->tdu_DiskIn == TDU_NODISK)
 934         return TDERR_DiskChanged;
 935     if (!td_getprotstatus(tdu, tdb)) {
 936         td_flush(tdu, tdb);
 937         err = td_format2(iotd, tdu, tdb);
 938     } else {
 939         err = TDERR_WriteProt;
 940     }
 941     return err;
 942 }
 943
 944 static UBYTE countbits(ULONG mask)
 945 {
 946     UBYTE cnt = 0;
 947     while (mask) {
 948         cnt++;
 949         mask >>= 1;
 950     }
 951     return cnt;
 952 }
 953
 954 void td_detectformat(struct TDU *tdu, struct TrackDiskBase *tdb)
 955 {
 956     UBYTE err;
 957     UBYTE track = 0;
 958     UBYTE cnt;
 959
 960     D(bug("detectformat HD=%d\n", tdu->tdu_hddisk ? 1 : 0));
 961     if (checkbuffer(tdu, tdb)) {
 962         tdu->tdu_disktype = DT_ADOS;
 963         tdu->tdu_sectors = tdu->tdu_hddisk ? 22 : 11;
 964         return;
 965     }
 966     tdu->tdu_disktype = DT_UNDETECTED;
 967     tdb->td_sectorbits = 0;
 968     tdb->td_buffer_unit = tdu->tdu_UnitNum;
 969     tdb->td_buffer_track = track;
 970     td_select(tdu, tdb);
 971     td_seek(tdu, track >> 1, track & 1, tdb);
 972     tdu->tdu_sectors = tdu->tdu_hddisk ? 22 : 11;
 973     err = td_readwritetrack(track, 0, tdu, tdb);
 974     if (!err) {
 975         err = td_decodebuffer_ados(tdu, tdb);
 976         /* Did all sectors fail to decode? It could be non-ADOS disk */
 977         if (err && tdb->td_sectorbits == 0) {
 978             tdu->tdu_sectors = tdu->tdu_hddisk ? 18 : 9;
 979             err = td_decodebuffer_pcdos(tdu, tdb);
 980             cnt = countbits(tdb->td_sectorbits);
 981             if (cnt > tdu->tdu_sectors / 2 + 1) {
 982                 /* enough sectors decoded fine, assume PCDOS */
 983                 tdu->tdu_disktype = DT_PCDOS;
 984             }
 985         } else {
 986             tdu->tdu_disktype = DT_ADOS;
 987         }
 988     }
 989     if (tdu->tdu_disktype == DT_UNDETECTED) {
 990         tdu->tdu_disktype = DT_ADOS;
 991         tdu->tdu_sectors = tdu->tdu_hddisk ? 22 : 11;
 992         tdb->td_sectorbits = 0;
 993     }
 994     D(bug("detectformat=%d sectors=%d sectormask=%08x\n", tdu->tdu_disktype, tdu->tdu_sectors, tdb->td_sectorbits));
 995  }