1 /* $NetBSD: fd.c,v 1.96 2015/04/26 15:15:19 mlelstv Exp $ */ 2 3 /* 4 * Copyright (c) 1994 Christian E. Hopps 5 * Copyright (c) 1996 Ezra Story 6 * All rights reserved. 7 * 8 * Redistribution and use in source and binary forms, with or without 9 * modification, are permitted provided that the following conditions 10 * are met: 11 * 1. Redistributions of source code must retain the above copyright 12 * notice, this list of conditions and the following disclaimer. 13 * 2. Redistributions in binary form must reproduce the above copyright 14 * notice, this list of conditions and the following disclaimer in the 15 * documentation and/or other materials provided with the distribution. 16 * 3. All advertising materials mentioning features or use of this software 17 * must display the following acknowledgement: 18 * This product includes software developed by Christian E. Hopps. 19 * This product includes software developed by Ezra Story. 20 * 4. The name of the author may not be used to endorse or promote products 21 * derived from this software without specific prior written permission 22 * 23 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR 24 * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES 25 * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. 26 * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, 27 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT 28 * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 29 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 30 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 31 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF 32 * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 33 */ 34 35 #include <sys/cdefs.h> 36 __KERNEL_RCSID(0, "$NetBSD: fd.c,v 1.96 2015/04/26 15:15:19 mlelstv Exp $"); 37 38 #include <sys/param.h> 39 #include <sys/systm.h> 40 #include <sys/callout.h> 41 #include <sys/kernel.h> 42 #include <sys/malloc.h> 43 #include <sys/buf.h> 44 #include <sys/bufq.h> 45 #include <sys/device.h> 46 #include <sys/ioctl.h> 47 #include <sys/fcntl.h> 48 #include <sys/disklabel.h> 49 #include <sys/disk.h> 50 #include <sys/dkbad.h> 51 #include <sys/proc.h> 52 #include <sys/conf.h> 53 54 #include <machine/cpu.h> 55 #include <amiga/amiga/device.h> 56 #include <amiga/amiga/custom.h> 57 #include <amiga/amiga/cia.h> 58 #include <amiga/amiga/cc.h> 59 60 #include "locators.h" 61 62 enum fdc_bits { FDB_CHANGED = 2, FDB_PROTECT, FDB_CYLZERO, FDB_READY }; 63 /* 64 * partitions in fd represent different format floppies 65 * partition a is 0 etc.. 66 */ 67 enum fd_parttypes { 68 FDAMIGAPART = 0, 69 FDMSDOSPART, 70 FDMAXPARTS 71 }; 72 73 #define FDBBSIZE (8192) 74 #define FDSBSIZE (8192) 75 76 #define FDUNIT(dev) DISKUNIT(dev) 77 #define FDPART(dev) DISKPART(dev) 78 #define FDMAKEDEV(m, u, p) MAKEDISKDEV((m), (u), (p)) 79 80 /* that's nice, but we don't want to always use this as an amiga drive 81 bunghole :-) */ 82 #define FDNHEADS (2) /* amiga drives always have 2 heads */ 83 #define FDSECSIZE (512) /* amiga drives always have 512 byte sectors */ 84 #define FDSECLWORDS (128) 85 86 #define FDSETTLEDELAY (18000) /* usec delay after seeking after switch dir */ 87 #define FDSTEPDELAY (3500) /* usec delay after steping */ 88 #define FDPRESIDEDELAY (1000) /* usec delay before writing can occur */ 89 #define FDWRITEDELAY (1300) /* usec delay after write */ 90 91 #define FDSTEPOUT (1) /* decrease track step */ 92 #define FDSTEPIN (0) /* increase track step */ 93 94 #define FDCUNITMASK (0x78) /* mask for all units (bits 6-3) */ 95 96 #define FDRETRIES (2) /* default number of retries */ 97 #define FDMAXUNITS (4) /* maximum number of supported units */ 98 99 #define DISKLEN_READ (0) /* fake mask for reading */ 100 #define DISKLEN_WRITE (1 << 14) /* bit for writing */ 101 #define DISKLEN_DMAEN (1 << 15) /* DMA go */ 102 #define DMABUFSZ ((DISKLEN_WRITE - 1) * 2) /* largest DMA possible */ 103 104 #define FDMFMSYNC (0x4489) 105 #define FDMFMID (0x5554) 106 #define FDMFMDATA (0x5545) 107 #define FDMFMGAP1 (0x9254) 108 #define FDMFMGAP2 (0xAAAA) 109 #define FDMFMGAP3 (0x9254) 110 #define CRC16POLY (0x1021) /* (x^16) + x^12 + x^5 + x^0 */ 111 112 /* 113 * Msdos-type MFM encode/decode 114 */ 115 static u_char msdecode[128]; 116 static u_char msencode[16] = 117 { 118 0x2a, 0x29, 0x24, 0x25, 0x12, 0x11, 0x14, 0x15, 119 0x4a, 0x49, 0x44, 0x45, 0x52, 0x51, 0x54, 0x55 120 }; 121 static u_short mscrctab[256]; 122 123 /* 124 5554 aaaa aaaa aaa5 2aa4 4452 aa51 125 00 00 03 02 ac 0d 126 */ 127 128 /* 129 * floppy device type 130 */ 131 struct fdtype { 132 u_int driveid; /* drive identification (from drive) */ 133 u_int ncylinders; /* number of cylinders on drive */ 134 u_int amiga_nsectors; /* number of sectors per amiga track */ 135 u_int msdos_nsectors; /* number of sectors per msdos track */ 136 u_int nreadw; /* number of words (short) read per track */ 137 u_int nwritew; /* number of words (short) written per track */ 138 u_int gap; /* track gap size in long words */ 139 const u_int precomp[2]; /* 1st and 2nd precomp values */ 140 const char *desc; /* description of drive type (useq) */ 141 }; 142 143 /* 144 * floppy disk device data 145 */ 146 struct fd_softc { 147 device_t sc_dev; /* generic device info; must come first */ 148 struct disk dkdev; /* generic disk info */ 149 struct bufq_state *bufq;/* queue pending I/O operations */ 150 struct buf curbuf; /* state of current I/O operation */ 151 struct callout calibrate_ch; 152 struct callout motor_ch; 153 struct fdtype *type; 154 void *cachep; /* cached track data (write through) */ 155 int cachetrk; /* cahced track -1 for none */ 156 int hwunit; /* unit for amiga controlling hw */ 157 int unitmask; /* mask for cia select deslect */ 158 int pstepdir; /* previous step direction */ 159 int curcyl; /* current curcyl head positioned on */ 160 int flags; /* misc flags */ 161 int wlabel; 162 int stepdelay; /* useq to delay after seek user setable */ 163 int nsectors; /* number of sectors per track */ 164 int openpart; /* which partition [ab] == [12] is open */ 165 short retries; /* number of times to retry failed io */ 166 short retried; /* number of times current io retried */ 167 int bytespersec; /* number of bytes per sector */ 168 }; 169 170 /* fd_softc->flags */ 171 #define FDF_MOTORON (0x01) /* motor is running */ 172 #define FDF_MOTOROFF (0x02) /* motor is waiting to be turned off */ 173 #define FDF_WMOTOROFF (0x04) /* unit wants a wakeup after off */ 174 #define FDF_DIRTY (0x08) /* track cache needs write */ 175 #define FDF_WRITEWAIT (0x10) /* need to head select delay on next setpos */ 176 #define FDF_HAVELABEL (0x20) /* label is valid */ 177 #define FDF_JUSTFLUSH (0x40) /* don't bother caching track. */ 178 #define FDF_NOTRACK0 (0x80) /* was not able to recalibrate drive */ 179 180 int fdc_wantwakeup; 181 int fdc_side; 182 void *fdc_dmap; 183 struct fd_softc *fdc_indma; 184 int fdc_dmalen; 185 int fdc_dmawrite; 186 187 struct fdcargs { 188 struct fdtype *type; 189 int unit; 190 }; 191 192 int fdcmatch(device_t, cfdata_t, void *); 193 void fdcattach(device_t, device_t, void *); 194 int fdcprint(void *, const char *); 195 int fdmatch(device_t, cfdata_t, void *); 196 void fdattach(device_t, device_t, void *); 197 198 void fdintr(int); 199 void fdidxintr(void); 200 int fdloaddisk(struct fd_softc *); 201 void fdgetdefaultlabel(struct fd_softc *, struct disklabel *, int); 202 int fdgetdisklabel(struct fd_softc *, dev_t); 203 int fdsetdisklabel(struct fd_softc *, struct disklabel *); 204 int fdputdisklabel(struct fd_softc *, dev_t); 205 struct fdtype * fdcgetfdtype(int); 206 void fdmotoroff(void *); 207 void fdsetpos(struct fd_softc *, int, int); 208 void fdselunit(struct fd_softc *); 209 void fdstart(struct fd_softc *); 210 void fdcont(struct fd_softc *); 211 void fddmastart(struct fd_softc *, int); 212 void fdcalibrate(void *); 213 void fddmadone(struct fd_softc *, int); 214 void fddone(struct fd_softc *); 215 void fdfindwork(int); 216 void fdminphys(struct buf *); 217 void fdcachetoraw(struct fd_softc *); 218 void amcachetoraw(struct fd_softc *); 219 int amrawtocache(struct fd_softc *); 220 u_long *fdfindsync(u_long *, u_long *); 221 int fdrawtocache(struct fd_softc *); 222 void mscachetoraw(struct fd_softc *); 223 int msrawtocache(struct fd_softc *); 224 u_long *mfmblkencode(u_long *, u_long *, u_long *, int); 225 u_long *mfmblkdecode(u_long *, u_long *, u_long *, int); 226 u_short *msblkdecode(u_short *, u_char *, int); 227 u_short *msblkencode(u_short *, u_char *, int, u_short *); 228 229 /* 230 * read size is (nsectors + 1) * mfm secsize + gap bytes + 2 shorts 231 * write size is nsectors * mfm secsize + gap bytes + 3 shorts 232 * the extra shorts are to deal with a DMA hw bug in the controller 233 * they are probably too much (I belive the bug is 1 short on write and 234 * 3 bits on read) but there is no need to be cheap here. 235 */ 236 #define MAXTRKSZ (22 * FDSECSIZE) 237 struct fdtype fdtype[] = { 238 { 0x00000000, 80, 11, 9, 7358, 6815, 414, { 80, 161 }, "3.5dd" }, 239 { 0x55555555, 40, 11, 9, 7358, 6815, 414, { 80, 161 }, "5.25dd" }, 240 { 0xAAAAAAAA, 80, 22, 18, 14716, 13630, 828, { 80, 161 }, "3.5hd" } 241 }; 242 int nfdtype = __arraycount(fdtype); 243 244 CFATTACH_DECL_NEW(fd, sizeof(struct fd_softc), 245 fdmatch, fdattach, NULL, NULL); 246 247 extern struct cfdriver fd_cd; 248 249 dev_type_open(fdopen); 250 dev_type_close(fdclose); 251 dev_type_read(fdread); 252 dev_type_write(fdwrite); 253 dev_type_ioctl(fdioctl); 254 dev_type_strategy(fdstrategy); 255 256 const struct bdevsw fd_bdevsw = { 257 .d_open = fdopen, 258 .d_close = fdclose, 259 .d_strategy = fdstrategy, 260 .d_ioctl = fdioctl, 261 .d_dump = nodump, 262 .d_psize = nosize, 263 .d_discard = nodiscard, 264 .d_flag = D_DISK 265 }; 266 267 const struct cdevsw fd_cdevsw = { 268 .d_open = fdopen, 269 .d_close = fdclose, 270 .d_read = fdread, 271 .d_write = fdwrite, 272 .d_ioctl = fdioctl, 273 .d_stop = nostop, 274 .d_tty = notty, 275 .d_poll = nopoll, 276 .d_mmap = nommap, 277 .d_kqfilter = nokqfilter, 278 .d_discard = nodiscard, 279 .d_flag = D_DISK 280 }; 281 282 struct dkdriver fddkdriver = { 283 .d_strategy = fdstrategy 284 }; 285 286 CFATTACH_DECL_NEW(fdc, 0, 287 fdcmatch, fdcattach, NULL, NULL); 288 289 /* 290 * all hw access through macros, this helps to hide the active low 291 * properties 292 */ 293 294 #define FDUNITMASK(unit) (1 << (3 + (unit))) 295 296 /* 297 * select units using mask 298 */ 299 #define FDSELECT(um) do { ciab.prb &= ~(um); } while (0) 300 301 /* 302 * deselect units using mask 303 */ 304 #define FDDESELECT(um) do { ciab.prb |= (um); delay(1); } while (0) 305 306 /* 307 * test hw condition bits 308 */ 309 #define FDTESTC(bit) ((ciaa.pra & (1 << (bit))) == 0) 310 311 /* 312 * set motor for select units, true motor on else off 313 */ 314 #define FDSETMOTOR(on) do { \ 315 if (on) ciab.prb &= ~CIAB_PRB_MTR; else ciab.prb |= CIAB_PRB_MTR; \ 316 } while (0) 317 318 /* 319 * set head for select units 320 */ 321 #define FDSETHEAD(head) do { \ 322 if (head) ciab.prb &= ~CIAB_PRB_SIDE; else ciab.prb |= CIAB_PRB_SIDE; \ 323 delay(1); } while (0) 324 325 /* 326 * select direction, true towards spindle else outwards 327 */ 328 #define FDSETDIR(in) do { \ 329 if (in) ciab.prb &= ~CIAB_PRB_DIR; else ciab.prb |= CIAB_PRB_DIR; \ 330 delay(1); } while (0) 331 332 /* 333 * step the selected units 334 */ 335 #define FDSTEP do { \ 336 ciab.prb &= ~CIAB_PRB_STEP; ciab.prb |= CIAB_PRB_STEP; \ 337 } while (0) 338 339 #define FDDMASTART(len, towrite) do { \ 340 int dmasz = (len) | ((towrite) ? DISKLEN_WRITE : 0) | DISKLEN_DMAEN; \ 341 custom.dsklen = dmasz; custom.dsklen = dmasz; } while (0) 342 343 #define FDDMASTOP do { custom.dsklen = 0; } while (0) 344 345 346 int 347 fdcmatch(device_t parent, cfdata_t cf, void *aux) 348 { 349 static int fdc_matched = 0; 350 351 /* Allow only once instance. */ 352 if (matchname("fdc", aux) == 0 || fdc_matched) 353 return(0); 354 if ((fdc_dmap = alloc_chipmem(DMABUFSZ)) == NULL) { 355 printf("fdc: unable to allocate DMA buffer\n"); 356 return(0); 357 } 358 359 fdc_matched = 1; 360 return(1); 361 } 362 363 void 364 fdcattach(device_t parent, device_t self, void *aux) 365 { 366 struct fdcargs args; 367 368 printf(": dmabuf pa 0x%x", (unsigned)kvtop(fdc_dmap)); 369 printf(": dmabuf ka %p\n", fdc_dmap); 370 args.unit = 0; 371 args.type = fdcgetfdtype(args.unit); 372 373 fdc_side = -1; 374 config_found(self, &args, fdcprint); 375 for (args.unit++; args.unit < FDMAXUNITS; args.unit++) { 376 if ((args.type = fdcgetfdtype(args.unit)) == NULL) 377 continue; 378 config_found(self, &args, fdcprint); 379 } 380 } 381 382 int 383 fdcprint(void *aux, const char *pnp) 384 { 385 struct fdcargs *fcp; 386 387 fcp = aux; 388 if (pnp) 389 aprint_normal("fd%d at %s unit %d:", fcp->unit, pnp, 390 fcp->type->driveid); 391 return(UNCONF); 392 } 393 394 /*ARGSUSED*/ 395 int 396 fdmatch(device_t parent, cfdata_t cf, void *aux) 397 { 398 struct fdcargs *fdap; 399 400 fdap = aux; 401 if (cf->cf_loc[FDCCF_UNIT] == fdap->unit || 402 cf->cf_loc[FDCCF_UNIT] == FDCCF_UNIT_DEFAULT) 403 return(1); 404 405 return(0); 406 } 407 408 void 409 fdattach(device_t parent, device_t self, void *aux) 410 { 411 struct fdcargs *ap; 412 struct fd_softc *sc; 413 int i; 414 415 ap = aux; 416 sc = device_private(self); 417 sc->sc_dev = self; 418 419 bufq_alloc(&sc->bufq, "disksort", BUFQ_SORT_CYLINDER); 420 callout_init(&sc->calibrate_ch, 0); 421 callout_init(&sc->motor_ch, 0); 422 423 sc->curcyl = sc->cachetrk = -1; 424 sc->openpart = -1; 425 sc->type = ap->type; 426 sc->hwunit = ap->unit; 427 sc->unitmask = 1 << (3 + ap->unit); 428 sc->retries = FDRETRIES; 429 sc->stepdelay = FDSTEPDELAY; 430 sc->bytespersec = 512; 431 printf(" unit %d: %s %d cyl, %d head, %d sec [%d sec], 512 bytes/sec\n", 432 sc->hwunit, sc->type->desc, sc->type->ncylinders, FDNHEADS, 433 sc->type->amiga_nsectors, sc->type->msdos_nsectors); 434 435 /* 436 * Initialize and attach the disk structure. 437 */ 438 disk_init(&sc->dkdev, device_xname(sc->sc_dev), &fddkdriver); 439 disk_attach(&sc->dkdev); 440 441 /* 442 * calibrate the drive 443 */ 444 fdsetpos(sc, 0, 0); 445 fdsetpos(sc, sc->type->ncylinders, 0); 446 fdsetpos(sc, 0, 0); 447 fdmotoroff(sc); 448 449 /* 450 * precalc msdos MFM and CRC 451 */ 452 for (i = 0; i < 128; i++) 453 msdecode[i] = 0xff; 454 for (i = 0; i < 16; i++) 455 msdecode[msencode[i]] = i; 456 for (i = 0; i < 256; i++) { 457 mscrctab[i] = (0x1021 * (i & 0xf0)) ^ (0x1021 * (i & 0x0f)) ^ 458 (0x1021 * (i >> 4)); 459 } 460 461 /* 462 * enable disk related interrupts 463 */ 464 custom.dmacon = DMAF_SETCLR | DMAF_MASTER | DMAF_DISK; 465 custom.intena = INTF_SETCLR | INTF_DSKBLK; 466 ciab.icr = CIA_ICR_FLG; 467 } 468 469 /*ARGSUSED*/ 470 int 471 fdopen(dev_t dev, int flags, int devtype, struct lwp *l) 472 { 473 struct fd_softc *sc; 474 int wasopen, fwork, error, s; 475 476 error = 0; 477 478 if (FDPART(dev) >= FDMAXPARTS) 479 return(ENXIO); 480 481 if ((sc = getsoftc(fd_cd, FDUNIT(dev))) == NULL) 482 return(ENXIO); 483 if (sc->flags & FDF_NOTRACK0) 484 return(ENXIO); 485 if (sc->cachep == NULL) 486 sc->cachep = malloc(MAXTRKSZ, M_DEVBUF, M_WAITOK); 487 488 s = splbio(); 489 /* 490 * if we are sleeping in fdclose(); waiting for a chance to 491 * shut the motor off, do a sleep here also. 492 */ 493 while (sc->flags & FDF_WMOTOROFF) 494 tsleep(fdmotoroff, PRIBIO, "fdopen", 0); 495 496 fwork = 0; 497 /* 498 * if not open let user open request type, otherwise 499 * ensure they are trying to open same type. 500 */ 501 if (sc->openpart == FDPART(dev)) 502 wasopen = 1; 503 else if (sc->openpart == -1) { 504 sc->openpart = FDPART(dev); 505 wasopen = 0; 506 } else { 507 wasopen = 1; 508 error = EPERM; 509 goto done; 510 } 511 512 /* 513 * wait for current io to complete if any 514 */ 515 if (fdc_indma) { 516 fwork = 1; 517 fdc_wantwakeup++; 518 tsleep(fdopen, PRIBIO, "fdopen", 0); 519 } 520 if ((error = fdloaddisk(sc)) != 0) 521 goto done; 522 if ((error = fdgetdisklabel(sc, dev)) != 0) 523 goto done; 524 #ifdef FDDEBUG 525 printf(" open successful\n"); 526 #endif 527 done: 528 /* 529 * if we requested that fddone()->fdfindwork() wake us, allow it to 530 * complete its job now 531 */ 532 if (fwork) 533 fdfindwork(FDUNIT(dev)); 534 splx(s); 535 536 /* 537 * if we were not open and we marked us so reverse that. 538 */ 539 if (error && wasopen == 0) 540 sc->openpart = -1; 541 return(error); 542 } 543 544 /*ARGSUSED*/ 545 int 546 fdclose(dev_t dev, int flags, int devtype, struct lwp *l) 547 { 548 struct fd_softc *sc; 549 int s; 550 551 #ifdef FDDEBUG 552 printf("fdclose()\n"); 553 #endif 554 sc = getsoftc(fd_cd, FDUNIT(dev)); 555 s = splbio(); 556 if (sc->flags & FDF_MOTORON) { 557 sc->flags |= FDF_WMOTOROFF; 558 tsleep(fdmotoroff, PRIBIO, "fdclose", 0); 559 sc->flags &= ~FDF_WMOTOROFF; 560 wakeup(fdmotoroff); 561 } 562 sc->openpart = -1; 563 splx(s); 564 return(0); 565 } 566 567 int 568 fdioctl(dev_t dev, u_long cmd, void *addr, int flag, struct lwp *l) 569 { 570 struct fd_softc *sc; 571 int error, wlab; 572 573 sc = getsoftc(fd_cd, FDUNIT(dev)); 574 575 if ((sc->flags & FDF_HAVELABEL) == 0) 576 return(EBADF); 577 578 error = disk_ioctl(&sc->dkdev, dev, cmd, addr, flag, l); 579 if (error != EPASSTHROUGH) 580 return error; 581 582 switch (cmd) { 583 case DIOCSBAD: 584 return(EINVAL); 585 case DIOCSRETRIES: 586 if (*(int *)addr < 0) 587 return(EINVAL); 588 sc->retries = *(int *)addr; 589 return(0); 590 case DIOCSSTEP: 591 if (*(int *)addr < FDSTEPDELAY) 592 return(EINVAL); 593 sc->dkdev.dk_label->d_trkseek = sc->stepdelay = *(int *)addr; 594 return(0); 595 case DIOCSDINFO: 596 if ((flag & FWRITE) == 0) 597 return(EBADF); 598 return(fdsetdisklabel(sc, (struct disklabel *)addr)); 599 case DIOCWDINFO: 600 if ((flag & FWRITE) == 0) 601 return(EBADF); 602 if ((error = fdsetdisklabel(sc, (struct disklabel *)addr)) != 0) 603 return(error); 604 wlab = sc->wlabel; 605 sc->wlabel = 1; 606 error = fdputdisklabel(sc, dev); 607 sc->wlabel = wlab; 608 return(error); 609 case DIOCWLABEL: 610 if ((flag & FWRITE) == 0) 611 return(EBADF); 612 sc->wlabel = *(int *)addr; 613 return(0); 614 case DIOCGDEFLABEL: 615 fdgetdefaultlabel(sc, (struct disklabel *)addr, FDPART(dev)); 616 return(0); 617 default: 618 return(ENOTTY); 619 } 620 } 621 622 int 623 fdread(dev_t dev, struct uio *uio, int flags) 624 { 625 return (physio(fdstrategy, NULL, dev, B_READ, fdminphys, uio)); 626 } 627 628 int 629 fdwrite(dev_t dev, struct uio *uio, int flags) 630 { 631 return (physio(fdstrategy, NULL, dev, B_WRITE, fdminphys, uio)); 632 } 633 634 635 void 636 fdintr(int flag) 637 { 638 int s; 639 640 s = splbio(); 641 if (fdc_indma) 642 fddmadone(fdc_indma, 0); 643 splx(s); 644 } 645 646 void 647 fdidxintr(void) 648 { 649 if (fdc_indma && fdc_dmalen) { 650 /* 651 * turn off intr and start actual dma 652 */ 653 ciab.icr = CIA_ICR_FLG; 654 FDDMASTART(fdc_dmalen, fdc_dmawrite); 655 fdc_dmalen = 0; 656 } 657 } 658 659 void 660 fdstrategy(struct buf *bp) 661 { 662 struct fd_softc *sc; 663 int unit, s; 664 665 unit = FDUNIT(bp->b_dev); 666 sc = getsoftc(fd_cd, unit); 667 668 #ifdef FDDEBUG 669 printf("fdstrategy: %p\n", bp); 670 #endif 671 /* 672 * check for valid partition and bounds 673 */ 674 if ((sc->flags & FDF_HAVELABEL) == 0) { 675 bp->b_error = EIO; 676 goto done; 677 } 678 if (bounds_check_with_label(&sc->dkdev, bp, sc->wlabel) <= 0) 679 goto done; 680 681 /* 682 * trans count of zero or bounds check indicates io is done 683 * we are done. 684 */ 685 if (bp->b_bcount == 0) 686 goto done; 687 688 bp->b_rawblkno = bp->b_blkno; 689 690 /* 691 * queue the buf and kick the low level code 692 */ 693 s = splbio(); 694 bufq_put(sc->bufq, bp); 695 fdstart(sc); 696 splx(s); 697 return; 698 done: 699 bp->b_resid = bp->b_bcount; 700 biodone(bp); 701 } 702 703 /* 704 * make sure disk is loaded and label is up-to-date. 705 */ 706 int 707 fdloaddisk(struct fd_softc *sc) 708 { 709 /* 710 * if diskchange is low step drive to 0 then up one then to zero. 711 */ 712 fdselunit(sc); /* make sure the unit is selected */ 713 if (FDTESTC(FDB_CHANGED)) { 714 fdsetpos(sc, 0, 0); 715 sc->cachetrk = -1; /* invalidate the cache */ 716 sc->flags &= ~FDF_HAVELABEL; 717 fdsetpos(sc, FDNHEADS, 0); 718 fdsetpos(sc, 0, 0); 719 if (FDTESTC(FDB_CHANGED)) { 720 fdmotoroff(sc); 721 FDDESELECT(sc->unitmask); 722 return(ENXIO); 723 } 724 } 725 FDDESELECT(sc->unitmask); 726 fdmotoroff(sc); 727 sc->type = fdcgetfdtype(sc->hwunit); 728 if (sc->type == NULL) 729 return(ENXIO); 730 if (sc->openpart == FDMSDOSPART) 731 sc->nsectors = sc->type->msdos_nsectors; 732 else 733 sc->nsectors = sc->type->amiga_nsectors; 734 return(0); 735 } 736 737 void 738 fdgetdefaultlabel(struct fd_softc *sc, struct disklabel *lp, int part) 739 /* (variable part) XXX ick */ 740 { 741 742 memset(lp, 0, sizeof(struct disklabel)); 743 lp->d_secsize = FDSECSIZE; 744 lp->d_ntracks = FDNHEADS; 745 lp->d_ncylinders = sc->type->ncylinders; 746 lp->d_nsectors = sc->nsectors; 747 lp->d_secpercyl = lp->d_ntracks * lp->d_nsectors; 748 lp->d_type = DKTYPE_FLOPPY; 749 lp->d_secperunit = lp->d_secpercyl * lp->d_ncylinders; 750 lp->d_rpm = 300; /* good guess I suppose. */ 751 lp->d_interleave = 1; /* should change when adding msdos */ 752 sc->stepdelay = lp->d_trkseek = FDSTEPDELAY; 753 lp->d_bbsize = 0; 754 lp->d_sbsize = 0; 755 lp->d_partitions[part].p_size = lp->d_secperunit; 756 lp->d_partitions[part].p_fstype = FS_UNUSED; 757 lp->d_partitions[part].p_fsize = 1024; 758 lp->d_partitions[part].p_frag = 8; 759 lp->d_partitions[part].p_cpg = 2; /* adosfs: reserved blocks */ 760 lp->d_npartitions = part + 1; 761 lp->d_magic = lp->d_magic2 = DISKMAGIC; 762 lp->d_checksum = dkcksum(lp); 763 } 764 765 /* 766 * read disk label, if present otherwise create one 767 * return a new label if raw part and none found, otherwise err. 768 */ 769 int 770 fdgetdisklabel(struct fd_softc *sc, dev_t dev) 771 { 772 struct disklabel *lp, *dlp; 773 struct cpu_disklabel *clp; 774 struct buf *bp; 775 int error, part; 776 777 if (sc->flags & FDF_HAVELABEL && 778 sc->dkdev.dk_label->d_npartitions == (FDPART(dev) + 1)) 779 return(0); 780 #ifdef FDDEBUG 781 printf("fdgetdisklabel()\n"); 782 #endif 783 part = FDPART(dev); 784 lp = sc->dkdev.dk_label; 785 clp = sc->dkdev.dk_cpulabel; 786 memset(lp, 0, sizeof(struct disklabel)); 787 memset(clp, 0, sizeof(struct cpu_disklabel)); 788 789 lp->d_secsize = FDSECSIZE; 790 lp->d_ntracks = FDNHEADS; 791 lp->d_ncylinders = sc->type->ncylinders; 792 lp->d_nsectors = sc->nsectors; 793 lp->d_secpercyl = lp->d_ntracks * lp->d_nsectors; 794 lp->d_secperunit = lp->d_secpercyl * lp->d_ncylinders; 795 lp->d_npartitions = part + 1; 796 lp->d_partitions[part].p_size = lp->d_secperunit; 797 lp->d_partitions[part].p_fstype = FS_UNUSED; 798 lp->d_partitions[part].p_fsize = 1024; 799 lp->d_partitions[part].p_frag = 8; 800 lp->d_partitions[part].p_cpg = 2; /* for adosfs: reserved blks */ 801 802 sc->flags |= FDF_HAVELABEL; 803 804 bp = (void *)geteblk((int)lp->d_secsize); 805 bp->b_dev = dev; 806 bp->b_blkno = 0; 807 bp->b_cylinder = 0; 808 bp->b_bcount = FDSECSIZE; 809 bp->b_flags |= B_READ; 810 fdstrategy(bp); 811 if ((error = biowait(bp)) != 0) 812 goto nolabel; 813 dlp = (struct disklabel *)((char*)bp->b_data + LABELOFFSET); 814 if (dlp->d_magic != DISKMAGIC || dlp->d_magic2 != DISKMAGIC || 815 dkcksum(dlp)) { 816 error = EINVAL; 817 goto nolabel; 818 } 819 memcpy(lp, dlp, sizeof(struct disklabel)); 820 if (lp->d_trkseek > FDSTEPDELAY) 821 sc->stepdelay = lp->d_trkseek; 822 brelse(bp, 0); 823 return(0); 824 nolabel: 825 fdgetdefaultlabel(sc, lp, part); 826 brelse(bp, 0); 827 return(0); 828 } 829 830 /* 831 * set the incore copy of this units disklabel 832 */ 833 int 834 fdsetdisklabel(struct fd_softc *sc, struct disklabel *lp) 835 { 836 struct disklabel *clp; 837 struct partition *pp; 838 839 /* 840 * must have at least opened raw unit to fetch the 841 * raw_part stuff. 842 */ 843 if ((sc->flags & FDF_HAVELABEL) == 0) 844 return(EINVAL); 845 clp = sc->dkdev.dk_label; 846 /* 847 * make sure things check out and we only have one valid 848 * partition 849 */ 850 #ifdef FDDEBUG 851 printf("fdsetdisklabel\n"); 852 #endif 853 if (lp->d_secsize != FDSECSIZE || 854 lp->d_nsectors != clp->d_nsectors || 855 lp->d_ntracks != FDNHEADS || 856 lp->d_ncylinders != clp->d_ncylinders || 857 lp->d_secpercyl != clp->d_secpercyl || 858 lp->d_secperunit != clp->d_secperunit || 859 lp->d_magic != DISKMAGIC || 860 lp->d_magic2 != DISKMAGIC || 861 lp->d_npartitions == 0 || 862 lp->d_npartitions > FDMAXPARTS || 863 (lp->d_partitions[0].p_offset && lp->d_partitions[1].p_offset) || 864 dkcksum(lp)) 865 return(EINVAL); 866 /* 867 * if any partitions are present make sure they 868 * represent the currently open type 869 */ 870 if ((pp = &lp->d_partitions[0])->p_size) { 871 if ((pp = &lp->d_partitions[1])->p_size == 0) 872 goto done; 873 else if (sc->openpart != 1) 874 return(EINVAL); 875 } else if (sc->openpart != 0) 876 return(EINVAL); 877 /* 878 * make sure selected partition is within bounds 879 * XXX on the second check, its to handle a bug in 880 * XXX the cluster routines as they require mutliples 881 * XXX of PAGE_SIZE currently 882 */ 883 if ((pp->p_offset + pp->p_size >= lp->d_secperunit) || 884 (pp->p_frag * pp->p_fsize % PAGE_SIZE)) 885 return(EINVAL); 886 done: 887 memcpy(clp, lp, sizeof(struct disklabel)); 888 return(0); 889 } 890 891 /* 892 * write out the incore copy of this units disklabel 893 */ 894 int 895 fdputdisklabel(struct fd_softc *sc, dev_t dev) 896 { 897 struct disklabel *lp, *dlp; 898 struct buf *bp; 899 int error; 900 901 if ((sc->flags & FDF_HAVELABEL) == 0) 902 return(EBADF); 903 #ifdef FDDEBUG 904 printf("fdputdisklabel\n"); 905 #endif 906 /* 907 * get buf and read in sector 0 908 */ 909 lp = sc->dkdev.dk_label; 910 bp = geteblk((int)lp->d_secsize); 911 bp->b_dev = FDMAKEDEV(major(dev), FDUNIT(dev), RAW_PART); 912 bp->b_blkno = 0; 913 bp->b_cylinder = 0; 914 bp->b_bcount = FDSECSIZE; 915 bp->b_flags |= B_READ; 916 fdstrategy(bp); 917 if ((error = biowait(bp)) != 0) 918 goto done; 919 /* 920 * copy disklabel to buf and write it out synchronous 921 */ 922 dlp = (struct disklabel *)((char*)bp->b_data + LABELOFFSET); 923 memcpy(dlp, lp, sizeof(struct disklabel)); 924 bp->b_blkno = 0; 925 bp->b_cylinder = 0; 926 bp->b_flags &= ~(B_READ); 927 bp->b_oflags &= ~(BO_DONE); 928 bp->b_flags |= B_WRITE; 929 fdstrategy(bp); 930 error = biowait(bp); 931 done: 932 brelse(bp, 0); 933 return(error); 934 } 935 936 /* 937 * figure out drive type or NULL if none. 938 */ 939 struct fdtype * 940 fdcgetfdtype(int unit) 941 { 942 struct fdtype *ftp; 943 u_long id, idb; 944 int cnt, umask; 945 946 id = 0; 947 umask = 1 << (3 + unit); 948 949 FDDESELECT(FDCUNITMASK); 950 951 FDSETMOTOR(1); 952 delay(1); 953 FDSELECT(umask); 954 delay(1); 955 FDDESELECT(umask); 956 957 FDSETMOTOR(0); 958 delay(1); 959 FDSELECT(umask); 960 delay(1); 961 FDDESELECT(umask); 962 963 for (idb = 0x80000000; idb; idb >>= 1) { 964 FDSELECT(umask); 965 delay(1); 966 if (FDTESTC(FDB_READY) == 0) 967 id |= idb; 968 FDDESELECT(umask); 969 delay(1); 970 } 971 #ifdef FDDEBUG 972 printf("fdcgettype unit %d id 0x%lx\n", unit, id); 973 #endif 974 975 for (cnt = 0, ftp = fdtype; cnt < nfdtype; ftp++, cnt++) 976 if (ftp->driveid == id) 977 return(ftp); 978 /* 979 * 3.5dd's at unit 0 do not always return id. 980 */ 981 if (unit == 0) 982 return(fdtype); 983 return(NULL); 984 } 985 986 /* 987 * turn motor off if possible otherwise mark as needed and will be done 988 * later. 989 */ 990 void 991 fdmotoroff(void *arg) 992 { 993 struct fd_softc *sc; 994 int s; 995 996 sc = arg; 997 s = splbio(); 998 999 #ifdef FDDEBUG 1000 printf("fdmotoroff: unit %d\n", sc->hwunit); 1001 #endif 1002 if ((sc->flags & FDF_MOTORON) == 0) 1003 goto done; 1004 /* 1005 * if we have a timeout on a DMA operation let fddmadone() 1006 * deal with it. 1007 */ 1008 if (fdc_indma == sc) { 1009 fddmadone(sc, 1); 1010 goto done; 1011 } 1012 #ifdef FDDEBUG 1013 printf(" motor was on, turning off\n"); 1014 #endif 1015 1016 /* 1017 * flush cache if needed 1018 */ 1019 if (sc->flags & FDF_DIRTY) { 1020 sc->flags |= FDF_JUSTFLUSH | FDF_MOTOROFF; 1021 #ifdef FDDEBUG 1022 printf(" flushing dirty buffer first\n"); 1023 #endif 1024 /* 1025 * if DMA'ing done for now, fddone() will call us again 1026 */ 1027 if (fdc_indma) 1028 goto done; 1029 fddmastart(sc, sc->cachetrk); 1030 goto done; 1031 } 1032 1033 /* 1034 * if controller is busy just schedule us to be called back 1035 */ 1036 if (fdc_indma) { 1037 /* 1038 * someone else has the controller now 1039 * just set flag and let fddone() call us again. 1040 */ 1041 sc->flags |= FDF_MOTOROFF; 1042 goto done; 1043 } 1044 1045 #ifdef FDDEBUG 1046 printf(" hw turning unit off\n"); 1047 #endif 1048 1049 sc->flags &= ~(FDF_MOTORON | FDF_MOTOROFF); 1050 FDDESELECT(FDCUNITMASK); 1051 FDSETMOTOR(0); 1052 delay(1); 1053 FDSELECT(sc->unitmask); 1054 delay(4); 1055 FDDESELECT(sc->unitmask); 1056 delay(1); 1057 if (sc->flags & FDF_WMOTOROFF) 1058 wakeup(fdmotoroff); 1059 done: 1060 splx(s); 1061 } 1062 1063 /* 1064 * select drive seek to track exit with motor on. 1065 * fdsetpos(x, 0, 0) does calibrates the drive. 1066 */ 1067 void 1068 fdsetpos(struct fd_softc *sc, int trk, int towrite) 1069 { 1070 int nstep, sdir, ondly, ncyl, nside; 1071 1072 FDDESELECT(FDCUNITMASK); 1073 FDSETMOTOR(1); 1074 delay(1); 1075 FDSELECT(sc->unitmask); 1076 delay(1); 1077 if ((sc->flags & FDF_MOTORON) == 0) { 1078 ondly = 0; 1079 while (FDTESTC(FDB_READY) == 0) { 1080 delay(1000); 1081 if (++ondly >= 1000) 1082 break; 1083 } 1084 } 1085 sc->flags |= FDF_MOTORON; 1086 1087 ncyl = trk / FDNHEADS; 1088 nside = trk % FDNHEADS; 1089 1090 if (sc->curcyl == ncyl && fdc_side == nside) 1091 return; 1092 1093 if (towrite) 1094 sc->flags |= FDF_WRITEWAIT; 1095 1096 #ifdef FDDEBUG 1097 printf("fdsetpos: cyl %d head %d towrite %d\n", trk / FDNHEADS, 1098 trk % FDNHEADS, towrite); 1099 #endif 1100 nstep = ncyl - sc->curcyl; 1101 if (nstep) { 1102 /* 1103 * figure direction 1104 */ 1105 if (nstep > 0 && ncyl != 0) { 1106 sdir = FDSTEPIN; 1107 FDSETDIR(1); 1108 } else { 1109 nstep = -nstep; 1110 sdir = FDSTEPOUT; 1111 FDSETDIR(0); 1112 } 1113 if (ncyl == 0) { 1114 /* 1115 * either just want cylinder 0 or doing 1116 * a calibrate. 1117 */ 1118 nstep = 256; 1119 while (FDTESTC(FDB_CYLZERO) == 0 && nstep--) { 1120 FDSTEP; 1121 delay(sc->stepdelay); 1122 } 1123 if (nstep < 0) 1124 sc->flags |= FDF_NOTRACK0; 1125 } else { 1126 /* 1127 * step the needed amount amount. 1128 */ 1129 while (nstep--) { 1130 FDSTEP; 1131 delay(sc->stepdelay); 1132 } 1133 } 1134 /* 1135 * if switched directions 1136 * allow drive to settle. 1137 */ 1138 if (sc->pstepdir != sdir) 1139 delay(FDSETTLEDELAY); 1140 sc->pstepdir = sdir; 1141 sc->curcyl = ncyl; 1142 } 1143 if (nside == fdc_side) 1144 return; 1145 /* 1146 * select side 1147 */ 1148 fdc_side = nside; 1149 FDSETHEAD(nside); 1150 delay(FDPRESIDEDELAY); 1151 } 1152 1153 void 1154 fdselunit(struct fd_softc *sc) 1155 { 1156 FDDESELECT(FDCUNITMASK); /* deselect all */ 1157 FDSETMOTOR(sc->flags & FDF_MOTORON); /* set motor to unit's state */ 1158 delay(1); 1159 FDSELECT(sc->unitmask); /* select unit */ 1160 delay(1); 1161 } 1162 1163 /* 1164 * process next buf on device queue. 1165 * normall sequence of events: 1166 * fdstart() -> fddmastart(); 1167 * fdidxintr(); 1168 * fdintr() -> fddmadone() -> fddone(); 1169 * if the track is in the cache then fdstart() will short-circuit 1170 * to fddone() else if the track cache is dirty it will flush. If 1171 * the buf is not an entire track it will cache the requested track. 1172 */ 1173 void 1174 fdstart(struct fd_softc *sc) 1175 { 1176 int trk, error, write; 1177 struct buf *bp, *dp; 1178 int changed; 1179 1180 #ifdef FDDEBUG 1181 printf("fdstart: unit %d\n", sc->hwunit); 1182 #endif 1183 1184 /* 1185 * if DMA'ing just return. we must have been called from fdstartegy. 1186 */ 1187 if (fdc_indma) 1188 return; 1189 1190 /* 1191 * get next buf if there. 1192 */ 1193 dp = &sc->curbuf; 1194 if ((bp = bufq_peek(sc->bufq)) == NULL) { 1195 #ifdef FDDEBUG 1196 printf(" nothing to do\n"); 1197 #endif 1198 return; 1199 } 1200 1201 /* 1202 * Mark us as busy now, in case fddone() gets called in one 1203 * of the cases below. 1204 */ 1205 disk_busy(&sc->dkdev); 1206 1207 /* 1208 * make sure same disk is loaded 1209 */ 1210 fdselunit(sc); 1211 changed = FDTESTC(FDB_CHANGED); 1212 FDDESELECT(sc->unitmask); 1213 if (changed) { 1214 /* 1215 * disk missing, invalidate all future io on 1216 * this unit until re-open()'ed also invalidate 1217 * all current io 1218 */ 1219 printf("fdstart: disk changed\n"); 1220 #ifdef FDDEBUG 1221 printf(" disk was removed invalidating all io\n"); 1222 #endif 1223 sc->flags &= ~FDF_HAVELABEL; 1224 for (;;) { 1225 bp = bufq_get(sc->bufq); 1226 bp->b_error = EIO; 1227 if (bufq_peek(sc->bufq) == NULL) 1228 break; 1229 biodone(bp); 1230 } 1231 /* 1232 * do fddone() on last buf to allow other units to start. 1233 */ 1234 bufq_put(sc->bufq, bp); 1235 fddone(sc); 1236 return; 1237 } 1238 1239 /* 1240 * we have a valid buf, setup our local version 1241 * we use this count to allow reading over multiple tracks. 1242 * into a single buffer 1243 */ 1244 dp->b_bcount = bp->b_bcount; 1245 dp->b_blkno = bp->b_blkno; 1246 dp->b_data = bp->b_data; 1247 dp->b_flags = bp->b_flags; 1248 dp->b_resid = 0; 1249 1250 if (bp->b_flags & B_READ) 1251 write = 0; 1252 else if (FDTESTC(FDB_PROTECT) == 0) 1253 write = 1; 1254 else { 1255 error = EPERM; 1256 goto done; 1257 } 1258 1259 /* 1260 * figure trk given blkno 1261 */ 1262 trk = bp->b_blkno / sc->nsectors; 1263 1264 /* 1265 * check to see if same as currently cached track 1266 * if so we need to do no DMA read. 1267 */ 1268 if (trk == sc->cachetrk) { 1269 fddone(sc); 1270 return; 1271 } 1272 1273 /* 1274 * if we will be overwriting the entire cache, don't bother to 1275 * fetch it. 1276 */ 1277 if (bp->b_bcount == (sc->nsectors * FDSECSIZE) && write && 1278 bp->b_blkno % sc->nsectors == 0) { 1279 if (sc->flags & FDF_DIRTY) 1280 sc->flags |= FDF_JUSTFLUSH; 1281 else { 1282 sc->cachetrk = trk; 1283 fddone(sc); 1284 return; 1285 } 1286 } 1287 1288 /* 1289 * start DMA read of `trk' 1290 */ 1291 fddmastart(sc, trk); 1292 return; 1293 done: 1294 bp->b_error = error; 1295 fddone(sc); 1296 } 1297 1298 /* 1299 * continue a started operation on next track. always begin at 1300 * sector 0 on the next track. 1301 */ 1302 void 1303 fdcont(struct fd_softc *sc) 1304 { 1305 struct buf *dp, *bp; 1306 int trk, write; 1307 1308 dp = &sc->curbuf; 1309 bp = bufq_peek(sc->bufq); 1310 dp->b_data = (char*)dp->b_data + (dp->b_bcount - bp->b_resid); 1311 dp->b_blkno += (dp->b_bcount - bp->b_resid) / FDSECSIZE; 1312 dp->b_bcount = bp->b_resid; 1313 1314 /* 1315 * figure trk given blkno 1316 */ 1317 trk = dp->b_blkno / sc->nsectors; 1318 #ifdef DEBUG 1319 if (trk != sc->cachetrk + 1 || dp->b_blkno % sc->nsectors != 0) 1320 panic("fdcont: confused"); 1321 #endif 1322 if (dp->b_flags & B_READ) 1323 write = 0; 1324 else 1325 write = 1; 1326 /* 1327 * if we will be overwriting the entire cache, don't bother to 1328 * fetch it. 1329 */ 1330 if (dp->b_bcount == (sc->nsectors * FDSECSIZE) && write) { 1331 if (sc->flags & FDF_DIRTY) 1332 sc->flags |= FDF_JUSTFLUSH; 1333 else { 1334 sc->cachetrk = trk; 1335 fddone(sc); 1336 return; 1337 } 1338 } 1339 /* 1340 * start DMA read of `trk' 1341 */ 1342 fddmastart(sc, trk); 1343 return; 1344 } 1345 1346 void 1347 fddmastart(struct fd_softc *sc, int trk) 1348 { 1349 int adkmask, ndmaw, write, dmatrk; 1350 1351 #ifdef FDDEBUG 1352 printf("fddmastart: unit %d cyl %d head %d", sc->hwunit, 1353 trk / FDNHEADS, trk % FDNHEADS); 1354 #endif 1355 /* 1356 * flush the cached track if dirty else read requested track. 1357 */ 1358 if (sc->flags & FDF_DIRTY) { 1359 fdcachetoraw(sc); 1360 ndmaw = sc->type->nwritew; 1361 dmatrk = sc->cachetrk; 1362 write = 1; 1363 } else { 1364 ndmaw = sc->type->nreadw; 1365 dmatrk = trk; 1366 write = 0; 1367 } 1368 1369 #ifdef FDDEBUG 1370 printf(" %s", write ? " flushing cache\n" : " loading cache\n"); 1371 #endif 1372 sc->cachetrk = trk; 1373 fdc_indma = sc; 1374 fdsetpos(sc, dmatrk, write); 1375 1376 /* 1377 * setup dma stuff 1378 */ 1379 if (write == 0) { 1380 custom.adkcon = ADKF_MSBSYNC; 1381 custom.adkcon = ADKF_SETCLR | ADKF_WORDSYNC | ADKF_FAST; 1382 custom.dsksync = FDMFMSYNC; 1383 } else { 1384 custom.adkcon = ADKF_PRECOMP1 | ADKF_PRECOMP0 | ADKF_WORDSYNC | 1385 ADKF_MSBSYNC; 1386 adkmask = ADKF_SETCLR | ADKF_FAST | ADKF_MFMPREC; 1387 if (dmatrk >= sc->type->precomp[0]) 1388 adkmask |= ADKF_PRECOMP0; 1389 if (dmatrk >= sc->type->precomp[1]) 1390 adkmask |= ADKF_PRECOMP1; 1391 custom.adkcon = adkmask; 1392 } 1393 custom.dskpt = (u_char *)kvtop(fdc_dmap); 1394 1395 /* 1396 * If writing an MSDOS track, activate disk index pulse 1397 * interrupt, DMA will be started in the intr routine fdidxintr() 1398 * Otherwise, start the DMA here. 1399 */ 1400 if (write && sc->openpart == FDMSDOSPART) { 1401 fdc_dmalen = ndmaw; 1402 fdc_dmawrite = write; 1403 ciab.icr = CIA_ICR_IR_SC | CIA_ICR_FLG; 1404 } else { 1405 FDDMASTART(ndmaw, write); 1406 fdc_dmalen = 0; 1407 } 1408 1409 #ifdef FDDEBUG 1410 printf(" DMA started\n"); 1411 #endif 1412 } 1413 1414 /* 1415 * recalibrate the drive 1416 */ 1417 void 1418 fdcalibrate(void *arg) 1419 { 1420 struct fd_softc *sc; 1421 static int loopcnt; 1422 1423 sc = arg; 1424 1425 if (loopcnt == 0) { 1426 /* 1427 * seek cyl 0 1428 */ 1429 fdc_indma = sc; 1430 sc->stepdelay += 900; 1431 if (sc->cachetrk > 1) 1432 fdsetpos(sc, sc->cachetrk % FDNHEADS, 0); 1433 sc->stepdelay -= 900; 1434 } 1435 if (loopcnt++ & 1) 1436 fdsetpos(sc, sc->cachetrk, 0); 1437 else 1438 fdsetpos(sc, sc->cachetrk + FDNHEADS, 0); 1439 /* 1440 * trk++, trk, trk++, trk, trk++, trk, trk++, trk and DMA 1441 */ 1442 if (loopcnt < 8) 1443 callout_reset(&sc->calibrate_ch, hz / 8, fdcalibrate, sc); 1444 else { 1445 loopcnt = 0; 1446 fdc_indma = NULL; 1447 callout_reset(&sc->motor_ch, 3 * hz / 2, fdmotoroff, sc); 1448 fddmastart(sc, sc->cachetrk); 1449 } 1450 } 1451 1452 void 1453 fddmadone(struct fd_softc *sc, int timeo) 1454 { 1455 #ifdef FDDEBUG 1456 printf("fddmadone: unit %d, timeo %d\n", sc->hwunit, timeo); 1457 #endif 1458 fdc_indma = NULL; 1459 callout_stop(&sc->motor_ch); 1460 FDDMASTOP; 1461 1462 /* 1463 * guarantee the drive has been at current head and cyl 1464 * for at least FDWRITEDELAY after a write. 1465 */ 1466 if (sc->flags & FDF_WRITEWAIT) { 1467 delay(FDWRITEDELAY); 1468 sc->flags &= ~FDF_WRITEWAIT; 1469 } 1470 1471 if ((sc->flags & FDF_MOTOROFF) == 0) { 1472 /* 1473 * motor runs for 1.5 seconds after last DMA 1474 */ 1475 callout_reset(&sc->motor_ch, 3 * hz / 2, fdmotoroff, sc); 1476 } 1477 if (sc->flags & FDF_DIRTY) { 1478 /* 1479 * if buffer dirty, the last DMA cleaned it 1480 */ 1481 sc->flags &= ~FDF_DIRTY; 1482 if (timeo) 1483 aprint_error_dev(sc->sc_dev, 1484 "write of track cache timed out.\n"); 1485 if (sc->flags & FDF_JUSTFLUSH) { 1486 sc->flags &= ~FDF_JUSTFLUSH; 1487 /* 1488 * we are done DMA'ing 1489 */ 1490 fddone(sc); 1491 return; 1492 } 1493 /* 1494 * load the cache 1495 */ 1496 fddmastart(sc, sc->cachetrk); 1497 return; 1498 } 1499 #ifdef FDDEBUG 1500 else if (sc->flags & FDF_MOTOROFF) 1501 panic("fddmadone: FDF_MOTOROFF with no FDF_DIRTY"); 1502 #endif 1503 1504 /* 1505 * cache loaded decode it into cache buffer 1506 */ 1507 if (timeo == 0 && fdrawtocache(sc) == 0) 1508 sc->retried = 0; 1509 else { 1510 #ifdef FDDEBUG 1511 if (timeo) 1512 aprint_debug_dev(sc->sc_dev, 1513 "fddmadone: cache load timed out.\n"); 1514 #endif 1515 if (sc->retried >= sc->retries) { 1516 sc->retried = 0; 1517 sc->cachetrk = -1; 1518 } else { 1519 sc->retried++; 1520 /* 1521 * this will be restarted at end of calibrate loop. 1522 */ 1523 callout_stop(&sc->motor_ch); 1524 fdcalibrate(sc); 1525 return; 1526 } 1527 } 1528 fddone(sc); 1529 } 1530 1531 void 1532 fddone(struct fd_softc *sc) 1533 { 1534 struct buf *dp, *bp; 1535 char *data; 1536 int sz; 1537 1538 #ifdef FDDEBUG 1539 printf("fddone: unit %d\n", sc->hwunit); 1540 #endif 1541 /* 1542 * check to see if unit is just flushing the cache, 1543 * that is we have no io queued. 1544 */ 1545 if (sc->flags & FDF_MOTOROFF) 1546 goto nobuf; 1547 1548 dp = &sc->curbuf; 1549 if ((bp = bufq_peek(sc->bufq)) == NULL) 1550 panic ("fddone"); 1551 /* 1552 * check for an error that may have occurred 1553 * while getting the track. 1554 */ 1555 if (sc->cachetrk == -1) { 1556 sc->retried = 0; 1557 bp->b_error = EIO; 1558 } else if (bp->b_error == 0) { 1559 data = sc->cachep; 1560 /* 1561 * get offset of data in track cache and limit 1562 * the copy size to not exceed the cache's end. 1563 */ 1564 data += (dp->b_blkno % sc->nsectors) * FDSECSIZE; 1565 sz = sc->nsectors - dp->b_blkno % sc->nsectors; 1566 sz *= FDSECSIZE; 1567 sz = min(dp->b_bcount, sz); 1568 if (bp->b_flags & B_READ) 1569 memcpy(dp->b_data, data, sz); 1570 else { 1571 memcpy(data, dp->b_data, sz); 1572 sc->flags |= FDF_DIRTY; 1573 } 1574 bp->b_resid = dp->b_bcount - sz; 1575 if (bp->b_resid == 0) { 1576 bp->b_error = 0; 1577 } else { 1578 /* 1579 * not done yet need to read next track 1580 */ 1581 fdcont(sc); 1582 return; 1583 } 1584 } 1585 /* 1586 * remove from queue. 1587 */ 1588 (void)bufq_get(sc->bufq); 1589 1590 disk_unbusy(&sc->dkdev, (bp->b_bcount - bp->b_resid), 1591 (bp->b_flags & B_READ)); 1592 1593 biodone(bp); 1594 nobuf: 1595 fdfindwork(device_unit(sc->sc_dev)); 1596 } 1597 1598 void 1599 fdfindwork(int unit) 1600 { 1601 struct fd_softc *ssc, *sc; 1602 int i, last; 1603 1604 /* 1605 * first see if we have any fdopen()'s waiting 1606 */ 1607 if (fdc_wantwakeup) { 1608 wakeup(fdopen); 1609 fdc_wantwakeup--; 1610 return; 1611 } 1612 1613 /* 1614 * start next available unit, linear search from the next unit 1615 * wrapping and finally this unit. 1616 */ 1617 last = 0; 1618 ssc = NULL; 1619 for (i = unit + 1; last == 0; i++) { 1620 if (i == unit) 1621 last = 1; 1622 if (i >= fd_cd.cd_ndevs) { 1623 i = -1; 1624 continue; 1625 } 1626 if ((sc = device_lookup_private(&fd_cd, i)) == NULL) 1627 continue; 1628 1629 /* 1630 * if unit has requested to be turned off 1631 * and it has no buf's queued do it now 1632 */ 1633 if (sc->flags & FDF_MOTOROFF) { 1634 if (bufq_peek(sc->bufq) == NULL) 1635 fdmotoroff(sc); 1636 else { 1637 /* 1638 * we gained a buf request while 1639 * we waited, forget the motoroff 1640 */ 1641 sc->flags &= ~FDF_MOTOROFF; 1642 } 1643 /* 1644 * if we now have DMA unit must have needed 1645 * flushing, quit 1646 */ 1647 if (fdc_indma) 1648 return; 1649 } 1650 /* 1651 * if we have no start unit and the current unit has 1652 * io waiting choose this unit to start. 1653 */ 1654 if (ssc == NULL && bufq_peek(sc->bufq) != NULL) 1655 ssc = sc; 1656 } 1657 if (ssc) 1658 fdstart(ssc); 1659 } 1660 1661 /* 1662 * min byte count to whats left of the track in question 1663 */ 1664 void 1665 fdminphys(struct buf *bp) 1666 { 1667 struct fd_softc *sc; 1668 int sec, toff, tsz; 1669 1670 if ((sc = getsoftc(fd_cd, FDUNIT(bp->b_dev))) == NULL) 1671 panic("fdminphys: couldn't get softc"); 1672 1673 sec = bp->b_blkno % sc->nsectors; 1674 1675 toff = sec * FDSECSIZE; 1676 tsz = sc->nsectors * FDSECSIZE; 1677 #ifdef FDDEBUG 1678 printf("fdminphys: before %ld", bp->b_bcount); 1679 #endif 1680 bp->b_bcount = min(bp->b_bcount, tsz - toff); 1681 #ifdef FDDEBUG 1682 printf(" after %ld\n", bp->b_bcount); 1683 #endif 1684 minphys(bp); 1685 } 1686 1687 /* 1688 * encode the track cache into raw MFM ready for DMA 1689 * when we go to multiple disk formats, this will call type dependent 1690 * functions 1691 */ 1692 void fdcachetoraw(struct fd_softc *sc) 1693 { 1694 if (sc->openpart == FDMSDOSPART) 1695 mscachetoraw(sc); 1696 else 1697 amcachetoraw(sc); 1698 } 1699 1700 /* 1701 * decode raw MFM from DMA into units track cache. 1702 * when we go to multiple disk formats, this will call type dependent 1703 * functions 1704 */ 1705 int 1706 fdrawtocache(struct fd_softc *sc) 1707 { 1708 1709 if (sc->openpart == FDMSDOSPART) 1710 return(msrawtocache(sc)); 1711 else 1712 return(amrawtocache(sc)); 1713 } 1714 1715 void 1716 amcachetoraw(struct fd_softc *sc) 1717 { 1718 static u_long mfmnull[4]; 1719 u_long *rp, *crp, *dp, hcksum, dcksum, info; 1720 int sec, i; 1721 1722 rp = fdc_dmap; 1723 1724 /* 1725 * not yet one sector (- 1 long) gap. 1726 * for now use previous drivers values 1727 */ 1728 for (i = 0; i < sc->type->gap; i++) 1729 *rp++ = 0xaaaaaaaa; 1730 /* 1731 * process sectors 1732 */ 1733 dp = sc->cachep; 1734 info = 0xff000000 | (sc->cachetrk << 16) | sc->nsectors; 1735 for (sec = 0; sec < sc->nsectors; sec++, info += (1 << 8) - 1) { 1736 hcksum = dcksum = 0; 1737 /* 1738 * sector format 1739 * offset description 1740 *----------------------------------- 1741 * 0 null 1742 * 1 sync 1743 * oddbits evenbits 1744 *---------------------- 1745 * 2 3 [0xff]b [trk]b [sec]b [togap]b 1746 * 4-7 8-11 null 1747 * 12 13 header cksum [2-11] 1748 * 14 15 data cksum [16-271] 1749 * 16-143 144-271 data 1750 */ 1751 *rp = 0xaaaaaaaa; 1752 if (*(rp - 1) & 0x1) 1753 *rp &= 0x7fffffff; /* clock bit correction */ 1754 rp++; 1755 *rp++ = (FDMFMSYNC << 16) | FDMFMSYNC; 1756 rp = mfmblkencode(&info, rp, &hcksum, 1); 1757 rp = mfmblkencode(mfmnull, rp, &hcksum, 4); 1758 rp = mfmblkencode(&hcksum, rp, NULL, 1); 1759 1760 crp = rp; 1761 rp = mfmblkencode(dp, rp + 2, &dcksum, FDSECLWORDS); 1762 dp += FDSECLWORDS; 1763 crp = mfmblkencode(&dcksum, crp, NULL, 1); 1764 if (*(crp - 1) & 0x1) 1765 *crp &= 0x7fffffff; /* clock bit correction */ 1766 else if ((*crp & 0x40000000) == 0) 1767 *crp |= 0x80000000; 1768 } 1769 *rp = 0xaaa80000; 1770 if (*(rp - 1) & 0x1) 1771 *rp &= 0x7fffffff; 1772 } 1773 1774 u_long * 1775 fdfindsync(u_long *rp, u_long *ep) 1776 { 1777 u_short *sp; 1778 1779 sp = (u_short *)rp; 1780 while ((u_long *)sp < ep && *sp != FDMFMSYNC) 1781 sp++; 1782 while ((u_long *)sp < ep && *sp == FDMFMSYNC) 1783 sp++; 1784 if ((u_long *)sp < ep) 1785 return((u_long *)sp); 1786 return(NULL); 1787 } 1788 1789 int 1790 amrawtocache(struct fd_softc *sc) 1791 { 1792 u_long mfmnull[4]; 1793 u_long *dp, *rp, *erp, *crp, *srp, hcksum, dcksum, info, cktmp; 1794 int cnt, doagain; 1795 1796 doagain = 1; 1797 srp = rp = fdc_dmap; 1798 erp = (u_long *)((u_short *)rp + sc->type->nreadw); 1799 cnt = 0; 1800 again: 1801 if (doagain == 0 || (rp = srp = fdfindsync(srp, erp)) == NULL) { 1802 #ifdef DIAGNOSTIC 1803 aprint_error_dev(sc->sc_dev, "corrupted track (%d) data.\n", 1804 sc->cachetrk); 1805 #endif 1806 return(-1); 1807 } 1808 1809 /* 1810 * process sectors 1811 */ 1812 for (; cnt < sc->nsectors; cnt++) { 1813 hcksum = dcksum = 0; 1814 rp = mfmblkdecode(rp, &info, &hcksum, 1); 1815 rp = mfmblkdecode(rp, mfmnull, &hcksum, 4); 1816 rp = mfmblkdecode(rp, &cktmp, NULL, 1); 1817 if (cktmp != hcksum) { 1818 #ifdef FDDEBUG 1819 printf(" info 0x%lx hchksum 0x%lx trkhcksum 0x%lx\n", 1820 info, hcksum, cktmp); 1821 #endif 1822 goto again; 1823 } 1824 if (((info >> 16) & 0xff) != sc->cachetrk) { 1825 #ifdef DEBUG 1826 aprint_debug_dev(sc->sc_dev, 1827 "incorrect track found: 0x%lx %d\n", 1828 info, sc->cachetrk); 1829 #endif 1830 goto again; 1831 } 1832 #ifdef FDDEBUG 1833 printf(" info 0x%lx\n", info); 1834 #endif 1835 1836 rp = mfmblkdecode(rp, &cktmp, NULL, 1); 1837 dp = sc->cachep; 1838 dp += FDSECLWORDS * ((info >> 8) & 0xff); 1839 crp = mfmblkdecode(rp, dp, &dcksum, FDSECLWORDS); 1840 if (cktmp != dcksum) { 1841 #ifdef FDDEBUG 1842 printf(" info 0x%lx dchksum 0x%lx trkdcksum 0x%lx\n", 1843 info, dcksum, cktmp); 1844 #endif 1845 goto again; 1846 } 1847 1848 /* 1849 * if we are at gap then we can no longer be sure 1850 * of correct sync marks 1851 */ 1852 if ((info & 0xff) == 1) 1853 doagain = 1; 1854 else 1855 doagain = 0; 1856 srp = rp = fdfindsync(crp, erp); 1857 } 1858 return(0); 1859 } 1860 1861 void 1862 mscachetoraw(struct fd_softc *sc) 1863 { 1864 u_short *rp, *erp, crc; 1865 u_char *cp, tb[5]; 1866 int sec, i; 1867 1868 rp = (u_short *)fdc_dmap; 1869 erp = rp + sc->type->nwritew; 1870 cp = sc->cachep; 1871 1872 /* 1873 * initial track filler (828 * GAP1) 1874 */ 1875 for (i = 0; i < sc->type->gap; i++) { 1876 *rp++ = FDMFMGAP1; 1877 *rp++ = FDMFMGAP1; 1878 } 1879 1880 for (sec = 0; sec < sc->nsectors; sec++) { 1881 1882 /* 1883 * leading sector gap 1884 * (12 * GAP2) + (3 * SYNC) 1885 */ 1886 for (i = 0; i < 12; i++) 1887 *rp++ = FDMFMGAP2; 1888 *rp++ = FDMFMSYNC; 1889 *rp++ = FDMFMSYNC; 1890 *rp++ = FDMFMSYNC; 1891 1892 /* 1893 * sector information 1894 * (ID) + track + side + sector + sector size + CRC16 1895 */ 1896 *rp++ = FDMFMID; 1897 tb[0] = sc->cachetrk / FDNHEADS; 1898 tb[1] = sc->cachetrk % FDNHEADS; 1899 tb[2] = sec + 1; 1900 i = sc->bytespersec; 1901 tb[3] = i < 256 ? 0 : (i < 512 ? 1 : (i < 1024 ? 2 : 3)); 1902 rp = msblkencode(rp, tb, 4, &crc); 1903 tb[0] = crc >> 8; 1904 tb[1] = crc & 0xff; 1905 tb[2] = 0x4e; /* GAP1 decoded */ 1906 rp = msblkencode(rp, tb, 3, 0); 1907 1908 /* 1909 * sector info/data gap 1910 * (22 * GAP1) + (12 * GAP2) + (3 * SYNC) 1911 */ 1912 for (i = 0; i < 21; i++) 1913 *rp++ = FDMFMGAP1; 1914 for (i = 0; i < 12; i++) 1915 *rp++ = FDMFMGAP2; 1916 *rp++ = FDMFMSYNC; 1917 *rp++ = FDMFMSYNC; 1918 *rp++ = FDMFMSYNC; 1919 1920 /* 1921 * sector data 1922 * (DATA) + ...data... + CRC16 1923 */ 1924 *rp++ = FDMFMDATA; 1925 rp = msblkencode(rp, cp, sc->bytespersec, &crc); 1926 cp += sc->bytespersec; 1927 tb[0] = crc >> 8; 1928 tb[1] = crc & 0xff; 1929 tb[2] = 0x4e; /* GAP3 decoded */ 1930 rp = msblkencode(rp, tb, 3, 0); 1931 1932 /* 1933 * trailing sector gap 1934 * (80 * GAP3) 1935 */ 1936 for (i = 0; i < 79; i++) 1937 *rp++ = FDMFMGAP3; 1938 } 1939 1940 /* 1941 * fill rest of track with GAP3 1942 */ 1943 while (rp != erp) 1944 *rp++ = FDMFMGAP3; 1945 1946 } 1947 1948 int 1949 msrawtocache(struct fd_softc *sc) 1950 { 1951 u_short *rp, *erp; 1952 u_char tb[5], *cp; 1953 int ct, sec, retry; 1954 1955 rp = (u_short *)fdc_dmap; 1956 erp = rp + sc->type->nreadw; 1957 cp = sc->cachep; 1958 1959 for (ct = 0; ct < sc->nsectors; ct++) { 1960 retry = 1; 1961 do { 1962 /* 1963 * skip leading gap to sync 1964 */ 1965 if ((rp = (u_short *)fdfindsync((u_long *)rp, (u_long *)erp)) == NULL) { 1966 #ifdef DIAGNOSTIC 1967 aprint_normal_dev(sc->sc_dev, 1968 "corrupted track (%d) data.\n", 1969 sc->cachetrk); 1970 #endif 1971 return(-1); 1972 } 1973 1974 /* 1975 * Grab sector info 1976 */ 1977 if (*rp++ != FDMFMID) 1978 continue; 1979 rp = msblkdecode(rp, tb, 4); 1980 #ifdef FDDEBUG 1981 printf("sector id: sector %d, track %d, side %d," 1982 "bps %d\n", tb[2], tb[0], tb[1], 128 << tb[3]); 1983 #endif 1984 if ((tb[0] * FDNHEADS + tb[1]) != sc->cachetrk || 1985 tb[2] > sc->nsectors) 1986 continue; 1987 1988 sec = tb[2]; 1989 sc->bytespersec = 128 << tb[3]; 1990 rp += 2; /* skip CRC-16 */ 1991 1992 /* 1993 * skip gap and read in data 1994 */ 1995 if ((rp = (u_short *)fdfindsync((u_long *)rp, (u_long *)erp)) == NULL) 1996 return(-1); 1997 if (*rp++ != FDMFMDATA) 1998 continue; 1999 rp = msblkdecode(rp, cp + ((sec-1) * sc->bytespersec), 2000 sc->bytespersec); 2001 rp += 2; /* skip CRC-16 */ 2002 2003 retry = 0; 2004 } while (retry); 2005 } 2006 return(0); 2007 } 2008 2009 /* 2010 * encode len longwords of `dp' data in amiga mfm block format (`rp') 2011 * this format specified that the odd bits are at current pos and even 2012 * bits at len + current pos 2013 */ 2014 u_long * 2015 mfmblkencode(u_long *dp, u_long *rp, u_long *cp, int len) 2016 { 2017 u_long *sdp, *edp, d, dtmp, correct; 2018 2019 sdp = dp; 2020 edp = dp + len; 2021 2022 if (*(rp - 1) & 0x1) 2023 correct = 1; 2024 else 2025 correct = 0; 2026 /* 2027 * do odd bits 2028 */ 2029 while (dp < edp) { 2030 d = (*dp >> 1) & 0x55555555; /* remove clock bits */ 2031 dtmp = d ^ 0x55555555; 2032 d |= ((dtmp >> 1) | 0x80000000) & (dtmp << 1); 2033 /* 2034 * correct upper clock bit if needed 2035 */ 2036 if (correct) 2037 d &= 0x7fffffff; 2038 if (d & 0x1) 2039 correct = 1; 2040 else 2041 correct = 0; 2042 /* 2043 * do checksums and store in raw buffer 2044 */ 2045 if (cp) 2046 *cp ^= d; 2047 *rp++ = d; 2048 dp++; 2049 } 2050 /* 2051 * do even bits 2052 */ 2053 dp = sdp; 2054 while (dp < edp) { 2055 d = *dp & 0x55555555; /* remove clock bits */ 2056 dtmp = d ^ 0x55555555; 2057 d |= ((dtmp >> 1) | 0x80000000) & (dtmp << 1); 2058 /* 2059 * correct upper clock bit if needed 2060 */ 2061 if (correct) 2062 d &= 0x7fffffff; 2063 if (d & 0x1) 2064 correct = 1; 2065 else 2066 correct = 0; 2067 /* 2068 * do checksums and store in raw buffer 2069 */ 2070 if (cp) 2071 *cp ^= d; 2072 *rp++ = d; 2073 dp++; 2074 } 2075 if (cp) 2076 *cp &= 0x55555555; 2077 return(rp); 2078 } 2079 2080 /* 2081 * decode len longwords of `dp' data in amiga mfm block format (`rp') 2082 * this format specified that the odd bits are at current pos and even 2083 * bits at len + current pos 2084 */ 2085 u_long * 2086 mfmblkdecode(u_long *rp, u_long *dp, u_long *cp, int len) 2087 { 2088 u_long o, e; 2089 int cnt; 2090 2091 cnt = len; 2092 while (cnt--) { 2093 o = *rp; 2094 e = *(rp + len); 2095 if (cp) { 2096 *cp ^= o; 2097 *cp ^= e; 2098 } 2099 o &= 0x55555555; 2100 e &= 0x55555555; 2101 *dp++ = (o << 1) | e; 2102 rp++; 2103 } 2104 if (cp) 2105 *cp &= 0x55555555; 2106 return(rp + len); 2107 } 2108 2109 /* 2110 * decode len words in standard MFM format to len bytes 2111 * of data. 2112 */ 2113 u_short * 2114 msblkdecode(u_short *rp, u_char *cp, int len) 2115 { 2116 while (len--) { 2117 *cp++ = msdecode[*rp & 0x7f] | 2118 (msdecode[(*rp >> 8) & 0x7f] << 4); 2119 rp++; 2120 } 2121 2122 return(rp); 2123 } 2124 2125 /* 2126 * encode len bytes of data into len words in standard MFM format. 2127 * If a pointer is supplied for crc, calculate the CRC-16 of the data 2128 * as well. 2129 */ 2130 u_short * 2131 msblkencode(u_short *rp, u_char *cp, int len, u_short *crc) 2132 { 2133 u_short td; 2134 u_short mycrc; 2135 2136 /* preload crc for header (4 bytes) 2137 * or data (anything else) 2138 */ 2139 mycrc = (len == 4) ? 0xb230 : 0xe295; 2140 2141 while (len--) { 2142 td = (msencode[*cp >> 4] << 8) | msencode[*cp & 0x0f]; 2143 2144 /* Check for zeros in top bit of encode and bottom 2145 * bit of previous encode. if so, slap a one in betweem 2146 * them. 2147 */ 2148 if ((td & 0x140) == 0) 2149 td |= 0x80; 2150 if ((td & 0x4000) == 0 && (rp[-1] & 1) == 0) 2151 td |= 0x8000; 2152 2153 *rp++ = td; 2154 2155 /* 2156 * calc crc if requested 2157 */ 2158 if (crc) 2159 mycrc = (mycrc << 8) ^ mscrctab[*cp ^ (mycrc >> 8)]; 2160 2161 cp++; 2162 } 2163 2164 if (crc) 2165 *crc = mycrc; 2166 2167 return(rp); 2168 } 2169