fd.c revision 1.96.8.1
1 /* $NetBSD: fd.c,v 1.96.8.1 2017/04/27 05:36:31 pgoyette 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.8.1 2017/04/27 05:36:31 pgoyette 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 device_release(sc->sc_dev); 483 return(ENXIO); 484 } 485 if (sc->flags & FDF_NOTRACK0) { 486 device_release(sc->sc_dev) 487 return(ENXIO); 488 } 489 if (sc->cachep == NULL) 490 sc->cachep = malloc(MAXTRKSZ, M_DEVBUF, M_WAITOK); 491 492 s = splbio(); 493 /* 494 * if we are sleeping in fdclose(); waiting for a chance to 495 * shut the motor off, do a sleep here also. 496 */ 497 while (sc->flags & FDF_WMOTOROFF) 498 tsleep(fdmotoroff, PRIBIO, "fdopen", 0); 499 500 fwork = 0; 501 /* 502 * if not open let user open request type, otherwise 503 * ensure they are trying to open same type. 504 */ 505 if (sc->openpart == FDPART(dev)) 506 wasopen = 1; 507 else if (sc->openpart == -1) { 508 sc->openpart = FDPART(dev); 509 wasopen = 0; 510 } else { 511 wasopen = 1; 512 error = EPERM; 513 goto done; 514 } 515 516 /* 517 * wait for current io to complete if any 518 */ 519 if (fdc_indma) { 520 fwork = 1; 521 fdc_wantwakeup++; 522 tsleep(fdopen, PRIBIO, "fdopen", 0); 523 } 524 if ((error = fdloaddisk(sc)) != 0) 525 goto done; 526 if ((error = fdgetdisklabel(sc, dev)) != 0) 527 goto done; 528 #ifdef FDDEBUG 529 printf(" open successful\n"); 530 #endif 531 done: 532 /* 533 * if we requested that fddone()->fdfindwork() wake us, allow it to 534 * complete its job now 535 */ 536 if (fwork) 537 fdfindwork(FDUNIT(dev)); 538 splx(s); 539 540 /* 541 * if we were not open and we marked us so reverse that. 542 */ 543 if (error && wasopen == 0) 544 sc->openpart = -1; 545 device_release(sc->sc_dev); 546 return(error); 547 } 548 549 /*ARGSUSED*/ 550 int 551 fdclose(dev_t dev, int flags, int devtype, struct lwp *l) 552 { 553 struct fd_softc *sc; 554 int s; 555 556 #ifdef FDDEBUG 557 printf("fdclose()\n"); 558 #endif 559 sc = getsoftc(fd_cd, FDUNIT(dev)); 560 s = splbio(); 561 if (sc->flags & FDF_MOTORON) { 562 sc->flags |= FDF_WMOTOROFF; 563 tsleep(fdmotoroff, PRIBIO, "fdclose", 0); 564 sc->flags &= ~FDF_WMOTOROFF; 565 wakeup(fdmotoroff); 566 } 567 sc->openpart = -1; 568 splx(s); 569 device_release(sc->sc_dev); 570 return(0); 571 } 572 573 int 574 fdioctl(dev_t dev, u_long cmd, void *addr, int flag, struct lwp *l) 575 { 576 struct fd_softc *sc; 577 int error, wlab; 578 579 sc = getsoftc(fd_cd, FDUNIT(dev)); 580 581 if ((sc->flags & FDF_HAVELABEL) == 0) { 582 device_release(sc->sc_dev); 583 return(EBADF); 584 } 585 error = disk_ioctl(&sc->dkdev, dev, cmd, addr, flag, l); 586 if (error != EPASSTHROUGH) { 587 device_release(sc->sc_dev); 588 return error; 589 } 590 error = 0; 591 switch (cmd) { 592 case DIOCSBAD: 593 error = EINVAL; 594 break; 595 case DIOCSRETRIES: 596 if (*(int *)addr < 0) 597 error = EINVAL; 598 else 599 sc->retries = *(int *)addr; 600 break; 601 case DIOCSSTEP: 602 if (*(int *)addr < FDSTEPDELAY) { 603 error = EINVAL; 604 else 605 sc->dkdev.dk_label->d_trkseek = sc->stepdelay = 606 *(int *)addr; 607 break; 608 case DIOCSDINFO: 609 if ((flag & FWRITE) == 0) 610 error = EBADF; 611 else 612 error = fdsetdisklabel(sc, (struct disklabel *)addr); 613 break; 614 case DIOCWDINFO: 615 if ((flag & FWRITE) == 0) 616 error = EBADF; 617 else { 618 error = fdsetdisklabel(sc, (struct disklabel *)addr)); 619 if (error == 0) { 620 wlab = sc->wlabel; 621 sc->wlabel = 1; 622 error = fdputdisklabel(sc, dev); 623 sc->wlabel = wlab; 624 } 625 } 626 break; 627 case DIOCWLABEL: 628 if ((flag & FWRITE) == 0) 629 error = EBADF; 630 else 631 sc->wlabel = *(int *)addr; 632 break; 633 case DIOCGDEFLABEL: 634 fdgetdefaultlabel(sc, (struct disklabel *)addr, FDPART(dev)); 635 break; 636 default: 637 error = ENOTTY; 638 } 639 device_release(sc->sc_dev); 640 return error; 641 } 642 643 int 644 fdread(dev_t dev, struct uio *uio, int flags) 645 { 646 return (physio(fdstrategy, NULL, dev, B_READ, fdminphys, uio)); 647 } 648 649 int 650 fdwrite(dev_t dev, struct uio *uio, int flags) 651 { 652 return (physio(fdstrategy, NULL, dev, B_WRITE, fdminphys, uio)); 653 } 654 655 656 void 657 fdintr(int flag) 658 { 659 int s; 660 661 s = splbio(); 662 if (fdc_indma) 663 fddmadone(fdc_indma, 0); 664 splx(s); 665 } 666 667 void 668 fdidxintr(void) 669 { 670 if (fdc_indma && fdc_dmalen) { 671 /* 672 * turn off intr and start actual dma 673 */ 674 ciab.icr = CIA_ICR_FLG; 675 FDDMASTART(fdc_dmalen, fdc_dmawrite); 676 fdc_dmalen = 0; 677 } 678 } 679 680 void 681 fdstrategy(struct buf *bp) 682 { 683 struct fd_softc *sc; 684 int unit, s; 685 686 unit = FDUNIT(bp->b_dev); 687 sc = getsoftc(fd_cd, unit); 688 689 #ifdef FDDEBUG 690 printf("fdstrategy: %p\n", bp); 691 #endif 692 /* 693 * check for valid partition and bounds 694 */ 695 if ((sc->flags & FDF_HAVELABEL) == 0) { 696 bp->b_error = EIO; 697 goto done; 698 } 699 if (bounds_check_with_label(&sc->dkdev, bp, sc->wlabel) <= 0) 700 goto done; 701 702 /* 703 * trans count of zero or bounds check indicates io is done 704 * we are done. 705 */ 706 if (bp->b_bcount == 0) 707 goto done; 708 709 bp->b_rawblkno = bp->b_blkno; 710 711 /* 712 * queue the buf and kick the low level code 713 */ 714 s = splbio(); 715 bufq_put(sc->bufq, bp); 716 fdstart(sc); 717 splx(s); 718 device_release(sc->sc_dev); 719 return; 720 done: 721 bp->b_resid = bp->b_bcount; 722 biodone(bp); 723 device_release(sc->sc_dev); 724 } 725 726 /* 727 * make sure disk is loaded and label is up-to-date. 728 */ 729 int 730 fdloaddisk(struct fd_softc *sc) 731 { 732 /* 733 * if diskchange is low step drive to 0 then up one then to zero. 734 */ 735 fdselunit(sc); /* make sure the unit is selected */ 736 if (FDTESTC(FDB_CHANGED)) { 737 fdsetpos(sc, 0, 0); 738 sc->cachetrk = -1; /* invalidate the cache */ 739 sc->flags &= ~FDF_HAVELABEL; 740 fdsetpos(sc, FDNHEADS, 0); 741 fdsetpos(sc, 0, 0); 742 if (FDTESTC(FDB_CHANGED)) { 743 fdmotoroff(sc); 744 FDDESELECT(sc->unitmask); 745 return(ENXIO); 746 } 747 } 748 FDDESELECT(sc->unitmask); 749 fdmotoroff(sc); 750 sc->type = fdcgetfdtype(sc->hwunit); 751 if (sc->type == NULL) 752 return(ENXIO); 753 if (sc->openpart == FDMSDOSPART) 754 sc->nsectors = sc->type->msdos_nsectors; 755 else 756 sc->nsectors = sc->type->amiga_nsectors; 757 return(0); 758 } 759 760 void 761 fdgetdefaultlabel(struct fd_softc *sc, struct disklabel *lp, int part) 762 /* (variable part) XXX ick */ 763 { 764 765 memset(lp, 0, sizeof(struct disklabel)); 766 lp->d_secsize = FDSECSIZE; 767 lp->d_ntracks = FDNHEADS; 768 lp->d_ncylinders = sc->type->ncylinders; 769 lp->d_nsectors = sc->nsectors; 770 lp->d_secpercyl = lp->d_ntracks * lp->d_nsectors; 771 lp->d_type = DKTYPE_FLOPPY; 772 lp->d_secperunit = lp->d_secpercyl * lp->d_ncylinders; 773 lp->d_rpm = 300; /* good guess I suppose. */ 774 lp->d_interleave = 1; /* should change when adding msdos */ 775 sc->stepdelay = lp->d_trkseek = FDSTEPDELAY; 776 lp->d_bbsize = 0; 777 lp->d_sbsize = 0; 778 lp->d_partitions[part].p_size = lp->d_secperunit; 779 lp->d_partitions[part].p_fstype = FS_UNUSED; 780 lp->d_partitions[part].p_fsize = 1024; 781 lp->d_partitions[part].p_frag = 8; 782 lp->d_partitions[part].p_cpg = 2; /* adosfs: reserved blocks */ 783 lp->d_npartitions = part + 1; 784 lp->d_magic = lp->d_magic2 = DISKMAGIC; 785 lp->d_checksum = dkcksum(lp); 786 } 787 788 /* 789 * read disk label, if present otherwise create one 790 * return a new label if raw part and none found, otherwise err. 791 */ 792 int 793 fdgetdisklabel(struct fd_softc *sc, dev_t dev) 794 { 795 struct disklabel *lp, *dlp; 796 struct cpu_disklabel *clp; 797 struct buf *bp; 798 int error, part; 799 800 if (sc->flags & FDF_HAVELABEL && 801 sc->dkdev.dk_label->d_npartitions == (FDPART(dev) + 1)) 802 return(0); 803 #ifdef FDDEBUG 804 printf("fdgetdisklabel()\n"); 805 #endif 806 part = FDPART(dev); 807 lp = sc->dkdev.dk_label; 808 clp = sc->dkdev.dk_cpulabel; 809 memset(lp, 0, sizeof(struct disklabel)); 810 memset(clp, 0, sizeof(struct cpu_disklabel)); 811 812 lp->d_secsize = FDSECSIZE; 813 lp->d_ntracks = FDNHEADS; 814 lp->d_ncylinders = sc->type->ncylinders; 815 lp->d_nsectors = sc->nsectors; 816 lp->d_secpercyl = lp->d_ntracks * lp->d_nsectors; 817 lp->d_secperunit = lp->d_secpercyl * lp->d_ncylinders; 818 lp->d_npartitions = part + 1; 819 lp->d_partitions[part].p_size = lp->d_secperunit; 820 lp->d_partitions[part].p_fstype = FS_UNUSED; 821 lp->d_partitions[part].p_fsize = 1024; 822 lp->d_partitions[part].p_frag = 8; 823 lp->d_partitions[part].p_cpg = 2; /* for adosfs: reserved blks */ 824 825 sc->flags |= FDF_HAVELABEL; 826 827 bp = (void *)geteblk((int)lp->d_secsize); 828 bp->b_dev = dev; 829 bp->b_blkno = 0; 830 bp->b_cylinder = 0; 831 bp->b_bcount = FDSECSIZE; 832 bp->b_flags |= B_READ; 833 fdstrategy(bp); 834 if ((error = biowait(bp)) != 0) 835 goto nolabel; 836 dlp = (struct disklabel *)((char*)bp->b_data + LABELOFFSET); 837 if (dlp->d_magic != DISKMAGIC || dlp->d_magic2 != DISKMAGIC || 838 dkcksum(dlp)) { 839 error = EINVAL; 840 goto nolabel; 841 } 842 memcpy(lp, dlp, sizeof(struct disklabel)); 843 if (lp->d_trkseek > FDSTEPDELAY) 844 sc->stepdelay = lp->d_trkseek; 845 brelse(bp, 0); 846 return(0); 847 nolabel: 848 fdgetdefaultlabel(sc, lp, part); 849 brelse(bp, 0); 850 return(0); 851 } 852 853 /* 854 * set the incore copy of this units disklabel 855 */ 856 int 857 fdsetdisklabel(struct fd_softc *sc, struct disklabel *lp) 858 { 859 struct disklabel *clp; 860 struct partition *pp; 861 862 /* 863 * must have at least opened raw unit to fetch the 864 * raw_part stuff. 865 */ 866 if ((sc->flags & FDF_HAVELABEL) == 0) 867 return(EINVAL); 868 clp = sc->dkdev.dk_label; 869 /* 870 * make sure things check out and we only have one valid 871 * partition 872 */ 873 #ifdef FDDEBUG 874 printf("fdsetdisklabel\n"); 875 #endif 876 if (lp->d_secsize != FDSECSIZE || 877 lp->d_nsectors != clp->d_nsectors || 878 lp->d_ntracks != FDNHEADS || 879 lp->d_ncylinders != clp->d_ncylinders || 880 lp->d_secpercyl != clp->d_secpercyl || 881 lp->d_secperunit != clp->d_secperunit || 882 lp->d_magic != DISKMAGIC || 883 lp->d_magic2 != DISKMAGIC || 884 lp->d_npartitions == 0 || 885 lp->d_npartitions > FDMAXPARTS || 886 (lp->d_partitions[0].p_offset && lp->d_partitions[1].p_offset) || 887 dkcksum(lp)) 888 return(EINVAL); 889 /* 890 * if any partitions are present make sure they 891 * represent the currently open type 892 */ 893 if ((pp = &lp->d_partitions[0])->p_size) { 894 if ((pp = &lp->d_partitions[1])->p_size == 0) 895 goto done; 896 else if (sc->openpart != 1) 897 return(EINVAL); 898 } else if (sc->openpart != 0) 899 return(EINVAL); 900 /* 901 * make sure selected partition is within bounds 902 * XXX on the second check, its to handle a bug in 903 * XXX the cluster routines as they require mutliples 904 * XXX of PAGE_SIZE currently 905 */ 906 if ((pp->p_offset + pp->p_size >= lp->d_secperunit) || 907 (pp->p_frag * pp->p_fsize % PAGE_SIZE)) 908 return(EINVAL); 909 done: 910 memcpy(clp, lp, sizeof(struct disklabel)); 911 return(0); 912 } 913 914 /* 915 * write out the incore copy of this units disklabel 916 */ 917 int 918 fdputdisklabel(struct fd_softc *sc, dev_t dev) 919 { 920 struct disklabel *lp, *dlp; 921 struct buf *bp; 922 int error; 923 924 if ((sc->flags & FDF_HAVELABEL) == 0) 925 return(EBADF); 926 #ifdef FDDEBUG 927 printf("fdputdisklabel\n"); 928 #endif 929 /* 930 * get buf and read in sector 0 931 */ 932 lp = sc->dkdev.dk_label; 933 bp = geteblk((int)lp->d_secsize); 934 bp->b_dev = FDMAKEDEV(major(dev), FDUNIT(dev), RAW_PART); 935 bp->b_blkno = 0; 936 bp->b_cylinder = 0; 937 bp->b_bcount = FDSECSIZE; 938 bp->b_flags |= B_READ; 939 fdstrategy(bp); 940 if ((error = biowait(bp)) != 0) 941 goto done; 942 /* 943 * copy disklabel to buf and write it out synchronous 944 */ 945 dlp = (struct disklabel *)((char*)bp->b_data + LABELOFFSET); 946 memcpy(dlp, lp, sizeof(struct disklabel)); 947 bp->b_blkno = 0; 948 bp->b_cylinder = 0; 949 bp->b_flags &= ~(B_READ); 950 bp->b_oflags &= ~(BO_DONE); 951 bp->b_flags |= B_WRITE; 952 fdstrategy(bp); 953 error = biowait(bp); 954 done: 955 brelse(bp, 0); 956 return(error); 957 } 958 959 /* 960 * figure out drive type or NULL if none. 961 */ 962 struct fdtype * 963 fdcgetfdtype(int unit) 964 { 965 struct fdtype *ftp; 966 u_long id, idb; 967 int cnt, umask; 968 969 id = 0; 970 umask = 1 << (3 + unit); 971 972 FDDESELECT(FDCUNITMASK); 973 974 FDSETMOTOR(1); 975 delay(1); 976 FDSELECT(umask); 977 delay(1); 978 FDDESELECT(umask); 979 980 FDSETMOTOR(0); 981 delay(1); 982 FDSELECT(umask); 983 delay(1); 984 FDDESELECT(umask); 985 986 for (idb = 0x80000000; idb; idb >>= 1) { 987 FDSELECT(umask); 988 delay(1); 989 if (FDTESTC(FDB_READY) == 0) 990 id |= idb; 991 FDDESELECT(umask); 992 delay(1); 993 } 994 #ifdef FDDEBUG 995 printf("fdcgettype unit %d id 0x%lx\n", unit, id); 996 #endif 997 998 for (cnt = 0, ftp = fdtype; cnt < nfdtype; ftp++, cnt++) 999 if (ftp->driveid == id) 1000 return(ftp); 1001 /* 1002 * 3.5dd's at unit 0 do not always return id. 1003 */ 1004 if (unit == 0) 1005 return(fdtype); 1006 return(NULL); 1007 } 1008 1009 /* 1010 * turn motor off if possible otherwise mark as needed and will be done 1011 * later. 1012 */ 1013 void 1014 fdmotoroff(void *arg) 1015 { 1016 struct fd_softc *sc; 1017 int s; 1018 1019 sc = arg; 1020 s = splbio(); 1021 1022 #ifdef FDDEBUG 1023 printf("fdmotoroff: unit %d\n", sc->hwunit); 1024 #endif 1025 if ((sc->flags & FDF_MOTORON) == 0) 1026 goto done; 1027 /* 1028 * if we have a timeout on a DMA operation let fddmadone() 1029 * deal with it. 1030 */ 1031 if (fdc_indma == sc) { 1032 fddmadone(sc, 1); 1033 goto done; 1034 } 1035 #ifdef FDDEBUG 1036 printf(" motor was on, turning off\n"); 1037 #endif 1038 1039 /* 1040 * flush cache if needed 1041 */ 1042 if (sc->flags & FDF_DIRTY) { 1043 sc->flags |= FDF_JUSTFLUSH | FDF_MOTOROFF; 1044 #ifdef FDDEBUG 1045 printf(" flushing dirty buffer first\n"); 1046 #endif 1047 /* 1048 * if DMA'ing done for now, fddone() will call us again 1049 */ 1050 if (fdc_indma) 1051 goto done; 1052 fddmastart(sc, sc->cachetrk); 1053 goto done; 1054 } 1055 1056 /* 1057 * if controller is busy just schedule us to be called back 1058 */ 1059 if (fdc_indma) { 1060 /* 1061 * someone else has the controller now 1062 * just set flag and let fddone() call us again. 1063 */ 1064 sc->flags |= FDF_MOTOROFF; 1065 goto done; 1066 } 1067 1068 #ifdef FDDEBUG 1069 printf(" hw turning unit off\n"); 1070 #endif 1071 1072 sc->flags &= ~(FDF_MOTORON | FDF_MOTOROFF); 1073 FDDESELECT(FDCUNITMASK); 1074 FDSETMOTOR(0); 1075 delay(1); 1076 FDSELECT(sc->unitmask); 1077 delay(4); 1078 FDDESELECT(sc->unitmask); 1079 delay(1); 1080 if (sc->flags & FDF_WMOTOROFF) 1081 wakeup(fdmotoroff); 1082 done: 1083 splx(s); 1084 } 1085 1086 /* 1087 * select drive seek to track exit with motor on. 1088 * fdsetpos(x, 0, 0) does calibrates the drive. 1089 */ 1090 void 1091 fdsetpos(struct fd_softc *sc, int trk, int towrite) 1092 { 1093 int nstep, sdir, ondly, ncyl, nside; 1094 1095 FDDESELECT(FDCUNITMASK); 1096 FDSETMOTOR(1); 1097 delay(1); 1098 FDSELECT(sc->unitmask); 1099 delay(1); 1100 if ((sc->flags & FDF_MOTORON) == 0) { 1101 ondly = 0; 1102 while (FDTESTC(FDB_READY) == 0) { 1103 delay(1000); 1104 if (++ondly >= 1000) 1105 break; 1106 } 1107 } 1108 sc->flags |= FDF_MOTORON; 1109 1110 ncyl = trk / FDNHEADS; 1111 nside = trk % FDNHEADS; 1112 1113 if (sc->curcyl == ncyl && fdc_side == nside) 1114 return; 1115 1116 if (towrite) 1117 sc->flags |= FDF_WRITEWAIT; 1118 1119 #ifdef FDDEBUG 1120 printf("fdsetpos: cyl %d head %d towrite %d\n", trk / FDNHEADS, 1121 trk % FDNHEADS, towrite); 1122 #endif 1123 nstep = ncyl - sc->curcyl; 1124 if (nstep) { 1125 /* 1126 * figure direction 1127 */ 1128 if (nstep > 0 && ncyl != 0) { 1129 sdir = FDSTEPIN; 1130 FDSETDIR(1); 1131 } else { 1132 nstep = -nstep; 1133 sdir = FDSTEPOUT; 1134 FDSETDIR(0); 1135 } 1136 if (ncyl == 0) { 1137 /* 1138 * either just want cylinder 0 or doing 1139 * a calibrate. 1140 */ 1141 nstep = 256; 1142 while (FDTESTC(FDB_CYLZERO) == 0 && nstep--) { 1143 FDSTEP; 1144 delay(sc->stepdelay); 1145 } 1146 if (nstep < 0) 1147 sc->flags |= FDF_NOTRACK0; 1148 } else { 1149 /* 1150 * step the needed amount amount. 1151 */ 1152 while (nstep--) { 1153 FDSTEP; 1154 delay(sc->stepdelay); 1155 } 1156 } 1157 /* 1158 * if switched directions 1159 * allow drive to settle. 1160 */ 1161 if (sc->pstepdir != sdir) 1162 delay(FDSETTLEDELAY); 1163 sc->pstepdir = sdir; 1164 sc->curcyl = ncyl; 1165 } 1166 if (nside == fdc_side) 1167 return; 1168 /* 1169 * select side 1170 */ 1171 fdc_side = nside; 1172 FDSETHEAD(nside); 1173 delay(FDPRESIDEDELAY); 1174 } 1175 1176 void 1177 fdselunit(struct fd_softc *sc) 1178 { 1179 FDDESELECT(FDCUNITMASK); /* deselect all */ 1180 FDSETMOTOR(sc->flags & FDF_MOTORON); /* set motor to unit's state */ 1181 delay(1); 1182 FDSELECT(sc->unitmask); /* select unit */ 1183 delay(1); 1184 } 1185 1186 /* 1187 * process next buf on device queue. 1188 * normall sequence of events: 1189 * fdstart() -> fddmastart(); 1190 * fdidxintr(); 1191 * fdintr() -> fddmadone() -> fddone(); 1192 * if the track is in the cache then fdstart() will short-circuit 1193 * to fddone() else if the track cache is dirty it will flush. If 1194 * the buf is not an entire track it will cache the requested track. 1195 */ 1196 void 1197 fdstart(struct fd_softc *sc) 1198 { 1199 int trk, error, write; 1200 struct buf *bp, *dp; 1201 int changed; 1202 1203 #ifdef FDDEBUG 1204 printf("fdstart: unit %d\n", sc->hwunit); 1205 #endif 1206 1207 /* 1208 * if DMA'ing just return. we must have been called from fdstartegy. 1209 */ 1210 if (fdc_indma) 1211 return; 1212 1213 /* 1214 * get next buf if there. 1215 */ 1216 dp = &sc->curbuf; 1217 if ((bp = bufq_peek(sc->bufq)) == NULL) { 1218 #ifdef FDDEBUG 1219 printf(" nothing to do\n"); 1220 #endif 1221 return; 1222 } 1223 1224 /* 1225 * Mark us as busy now, in case fddone() gets called in one 1226 * of the cases below. 1227 */ 1228 disk_busy(&sc->dkdev); 1229 1230 /* 1231 * make sure same disk is loaded 1232 */ 1233 fdselunit(sc); 1234 changed = FDTESTC(FDB_CHANGED); 1235 FDDESELECT(sc->unitmask); 1236 if (changed) { 1237 /* 1238 * disk missing, invalidate all future io on 1239 * this unit until re-open()'ed also invalidate 1240 * all current io 1241 */ 1242 printf("fdstart: disk changed\n"); 1243 #ifdef FDDEBUG 1244 printf(" disk was removed invalidating all io\n"); 1245 #endif 1246 sc->flags &= ~FDF_HAVELABEL; 1247 for (;;) { 1248 bp = bufq_get(sc->bufq); 1249 bp->b_error = EIO; 1250 if (bufq_peek(sc->bufq) == NULL) 1251 break; 1252 biodone(bp); 1253 } 1254 /* 1255 * do fddone() on last buf to allow other units to start. 1256 */ 1257 bufq_put(sc->bufq, bp); 1258 fddone(sc); 1259 return; 1260 } 1261 1262 /* 1263 * we have a valid buf, setup our local version 1264 * we use this count to allow reading over multiple tracks. 1265 * into a single buffer 1266 */ 1267 dp->b_bcount = bp->b_bcount; 1268 dp->b_blkno = bp->b_blkno; 1269 dp->b_data = bp->b_data; 1270 dp->b_flags = bp->b_flags; 1271 dp->b_resid = 0; 1272 1273 if (bp->b_flags & B_READ) 1274 write = 0; 1275 else if (FDTESTC(FDB_PROTECT) == 0) 1276 write = 1; 1277 else { 1278 error = EPERM; 1279 goto done; 1280 } 1281 1282 /* 1283 * figure trk given blkno 1284 */ 1285 trk = bp->b_blkno / sc->nsectors; 1286 1287 /* 1288 * check to see if same as currently cached track 1289 * if so we need to do no DMA read. 1290 */ 1291 if (trk == sc->cachetrk) { 1292 fddone(sc); 1293 return; 1294 } 1295 1296 /* 1297 * if we will be overwriting the entire cache, don't bother to 1298 * fetch it. 1299 */ 1300 if (bp->b_bcount == (sc->nsectors * FDSECSIZE) && write && 1301 bp->b_blkno % sc->nsectors == 0) { 1302 if (sc->flags & FDF_DIRTY) 1303 sc->flags |= FDF_JUSTFLUSH; 1304 else { 1305 sc->cachetrk = trk; 1306 fddone(sc); 1307 return; 1308 } 1309 } 1310 1311 /* 1312 * start DMA read of `trk' 1313 */ 1314 fddmastart(sc, trk); 1315 return; 1316 done: 1317 bp->b_error = error; 1318 fddone(sc); 1319 } 1320 1321 /* 1322 * continue a started operation on next track. always begin at 1323 * sector 0 on the next track. 1324 */ 1325 void 1326 fdcont(struct fd_softc *sc) 1327 { 1328 struct buf *dp, *bp; 1329 int trk, write; 1330 1331 dp = &sc->curbuf; 1332 bp = bufq_peek(sc->bufq); 1333 dp->b_data = (char*)dp->b_data + (dp->b_bcount - bp->b_resid); 1334 dp->b_blkno += (dp->b_bcount - bp->b_resid) / FDSECSIZE; 1335 dp->b_bcount = bp->b_resid; 1336 1337 /* 1338 * figure trk given blkno 1339 */ 1340 trk = dp->b_blkno / sc->nsectors; 1341 #ifdef DEBUG 1342 if (trk != sc->cachetrk + 1 || dp->b_blkno % sc->nsectors != 0) 1343 panic("fdcont: confused"); 1344 #endif 1345 if (dp->b_flags & B_READ) 1346 write = 0; 1347 else 1348 write = 1; 1349 /* 1350 * if we will be overwriting the entire cache, don't bother to 1351 * fetch it. 1352 */ 1353 if (dp->b_bcount == (sc->nsectors * FDSECSIZE) && write) { 1354 if (sc->flags & FDF_DIRTY) 1355 sc->flags |= FDF_JUSTFLUSH; 1356 else { 1357 sc->cachetrk = trk; 1358 fddone(sc); 1359 return; 1360 } 1361 } 1362 /* 1363 * start DMA read of `trk' 1364 */ 1365 fddmastart(sc, trk); 1366 return; 1367 } 1368 1369 void 1370 fddmastart(struct fd_softc *sc, int trk) 1371 { 1372 int adkmask, ndmaw, write, dmatrk; 1373 1374 #ifdef FDDEBUG 1375 printf("fddmastart: unit %d cyl %d head %d", sc->hwunit, 1376 trk / FDNHEADS, trk % FDNHEADS); 1377 #endif 1378 /* 1379 * flush the cached track if dirty else read requested track. 1380 */ 1381 if (sc->flags & FDF_DIRTY) { 1382 fdcachetoraw(sc); 1383 ndmaw = sc->type->nwritew; 1384 dmatrk = sc->cachetrk; 1385 write = 1; 1386 } else { 1387 ndmaw = sc->type->nreadw; 1388 dmatrk = trk; 1389 write = 0; 1390 } 1391 1392 #ifdef FDDEBUG 1393 printf(" %s", write ? " flushing cache\n" : " loading cache\n"); 1394 #endif 1395 sc->cachetrk = trk; 1396 fdc_indma = sc; 1397 fdsetpos(sc, dmatrk, write); 1398 1399 /* 1400 * setup dma stuff 1401 */ 1402 if (write == 0) { 1403 custom.adkcon = ADKF_MSBSYNC; 1404 custom.adkcon = ADKF_SETCLR | ADKF_WORDSYNC | ADKF_FAST; 1405 custom.dsksync = FDMFMSYNC; 1406 } else { 1407 custom.adkcon = ADKF_PRECOMP1 | ADKF_PRECOMP0 | ADKF_WORDSYNC | 1408 ADKF_MSBSYNC; 1409 adkmask = ADKF_SETCLR | ADKF_FAST | ADKF_MFMPREC; 1410 if (dmatrk >= sc->type->precomp[0]) 1411 adkmask |= ADKF_PRECOMP0; 1412 if (dmatrk >= sc->type->precomp[1]) 1413 adkmask |= ADKF_PRECOMP1; 1414 custom.adkcon = adkmask; 1415 } 1416 custom.dskpt = (u_char *)kvtop(fdc_dmap); 1417 1418 /* 1419 * If writing an MSDOS track, activate disk index pulse 1420 * interrupt, DMA will be started in the intr routine fdidxintr() 1421 * Otherwise, start the DMA here. 1422 */ 1423 if (write && sc->openpart == FDMSDOSPART) { 1424 fdc_dmalen = ndmaw; 1425 fdc_dmawrite = write; 1426 ciab.icr = CIA_ICR_IR_SC | CIA_ICR_FLG; 1427 } else { 1428 FDDMASTART(ndmaw, write); 1429 fdc_dmalen = 0; 1430 } 1431 1432 #ifdef FDDEBUG 1433 printf(" DMA started\n"); 1434 #endif 1435 } 1436 1437 /* 1438 * recalibrate the drive 1439 */ 1440 void 1441 fdcalibrate(void *arg) 1442 { 1443 struct fd_softc *sc; 1444 static int loopcnt; 1445 1446 sc = arg; 1447 1448 if (loopcnt == 0) { 1449 /* 1450 * seek cyl 0 1451 */ 1452 fdc_indma = sc; 1453 sc->stepdelay += 900; 1454 if (sc->cachetrk > 1) 1455 fdsetpos(sc, sc->cachetrk % FDNHEADS, 0); 1456 sc->stepdelay -= 900; 1457 } 1458 if (loopcnt++ & 1) 1459 fdsetpos(sc, sc->cachetrk, 0); 1460 else 1461 fdsetpos(sc, sc->cachetrk + FDNHEADS, 0); 1462 /* 1463 * trk++, trk, trk++, trk, trk++, trk, trk++, trk and DMA 1464 */ 1465 if (loopcnt < 8) 1466 callout_reset(&sc->calibrate_ch, hz / 8, fdcalibrate, sc); 1467 else { 1468 loopcnt = 0; 1469 fdc_indma = NULL; 1470 callout_reset(&sc->motor_ch, 3 * hz / 2, fdmotoroff, sc); 1471 fddmastart(sc, sc->cachetrk); 1472 } 1473 } 1474 1475 void 1476 fddmadone(struct fd_softc *sc, int timeo) 1477 { 1478 #ifdef FDDEBUG 1479 printf("fddmadone: unit %d, timeo %d\n", sc->hwunit, timeo); 1480 #endif 1481 fdc_indma = NULL; 1482 callout_stop(&sc->motor_ch); 1483 FDDMASTOP; 1484 1485 /* 1486 * guarantee the drive has been at current head and cyl 1487 * for at least FDWRITEDELAY after a write. 1488 */ 1489 if (sc->flags & FDF_WRITEWAIT) { 1490 delay(FDWRITEDELAY); 1491 sc->flags &= ~FDF_WRITEWAIT; 1492 } 1493 1494 if ((sc->flags & FDF_MOTOROFF) == 0) { 1495 /* 1496 * motor runs for 1.5 seconds after last DMA 1497 */ 1498 callout_reset(&sc->motor_ch, 3 * hz / 2, fdmotoroff, sc); 1499 } 1500 if (sc->flags & FDF_DIRTY) { 1501 /* 1502 * if buffer dirty, the last DMA cleaned it 1503 */ 1504 sc->flags &= ~FDF_DIRTY; 1505 if (timeo) 1506 aprint_error_dev(sc->sc_dev, 1507 "write of track cache timed out.\n"); 1508 if (sc->flags & FDF_JUSTFLUSH) { 1509 sc->flags &= ~FDF_JUSTFLUSH; 1510 /* 1511 * we are done DMA'ing 1512 */ 1513 fddone(sc); 1514 return; 1515 } 1516 /* 1517 * load the cache 1518 */ 1519 fddmastart(sc, sc->cachetrk); 1520 return; 1521 } 1522 #ifdef FDDEBUG 1523 else if (sc->flags & FDF_MOTOROFF) 1524 panic("fddmadone: FDF_MOTOROFF with no FDF_DIRTY"); 1525 #endif 1526 1527 /* 1528 * cache loaded decode it into cache buffer 1529 */ 1530 if (timeo == 0 && fdrawtocache(sc) == 0) 1531 sc->retried = 0; 1532 else { 1533 #ifdef FDDEBUG 1534 if (timeo) 1535 aprint_debug_dev(sc->sc_dev, 1536 "fddmadone: cache load timed out.\n"); 1537 #endif 1538 if (sc->retried >= sc->retries) { 1539 sc->retried = 0; 1540 sc->cachetrk = -1; 1541 } else { 1542 sc->retried++; 1543 /* 1544 * this will be restarted at end of calibrate loop. 1545 */ 1546 callout_stop(&sc->motor_ch); 1547 fdcalibrate(sc); 1548 return; 1549 } 1550 } 1551 fddone(sc); 1552 } 1553 1554 void 1555 fddone(struct fd_softc *sc) 1556 { 1557 struct buf *dp, *bp; 1558 char *data; 1559 int sz; 1560 1561 #ifdef FDDEBUG 1562 printf("fddone: unit %d\n", sc->hwunit); 1563 #endif 1564 /* 1565 * check to see if unit is just flushing the cache, 1566 * that is we have no io queued. 1567 */ 1568 if (sc->flags & FDF_MOTOROFF) 1569 goto nobuf; 1570 1571 dp = &sc->curbuf; 1572 if ((bp = bufq_peek(sc->bufq)) == NULL) 1573 panic ("fddone"); 1574 /* 1575 * check for an error that may have occurred 1576 * while getting the track. 1577 */ 1578 if (sc->cachetrk == -1) { 1579 sc->retried = 0; 1580 bp->b_error = EIO; 1581 } else if (bp->b_error == 0) { 1582 data = sc->cachep; 1583 /* 1584 * get offset of data in track cache and limit 1585 * the copy size to not exceed the cache's end. 1586 */ 1587 data += (dp->b_blkno % sc->nsectors) * FDSECSIZE; 1588 sz = sc->nsectors - dp->b_blkno % sc->nsectors; 1589 sz *= FDSECSIZE; 1590 sz = min(dp->b_bcount, sz); 1591 if (bp->b_flags & B_READ) 1592 memcpy(dp->b_data, data, sz); 1593 else { 1594 memcpy(data, dp->b_data, sz); 1595 sc->flags |= FDF_DIRTY; 1596 } 1597 bp->b_resid = dp->b_bcount - sz; 1598 if (bp->b_resid == 0) { 1599 bp->b_error = 0; 1600 } else { 1601 /* 1602 * not done yet need to read next track 1603 */ 1604 fdcont(sc); 1605 return; 1606 } 1607 } 1608 /* 1609 * remove from queue. 1610 */ 1611 (void)bufq_get(sc->bufq); 1612 1613 disk_unbusy(&sc->dkdev, (bp->b_bcount - bp->b_resid), 1614 (bp->b_flags & B_READ)); 1615 1616 biodone(bp); 1617 nobuf: 1618 fdfindwork(device_unit(sc->sc_dev)); 1619 } 1620 1621 void 1622 fdfindwork(int unit) 1623 { 1624 struct fd_softc *ssc, *sc; 1625 int i, last; 1626 1627 /* 1628 * first see if we have any fdopen()'s waiting 1629 */ 1630 if (fdc_wantwakeup) { 1631 wakeup(fdopen); 1632 fdc_wantwakeup--; 1633 return; 1634 } 1635 1636 /* 1637 * start next available unit, linear search from the next unit 1638 * wrapping and finally this unit. 1639 */ 1640 last = 0; 1641 ssc = NULL; 1642 for (i = unit + 1; last == 0; i++) { 1643 if (i == unit) 1644 last = 1; 1645 if (i >= fd_cd.cd_ndevs) { 1646 i = -1; 1647 continue; 1648 } 1649 if ((sc = device_lookup_private_acquire(&fd_cd, i)) == NULL) 1650 continue; 1651 1652 /* 1653 * if unit has requested to be turned off 1654 * and it has no buf's queued do it now 1655 */ 1656 if (sc->flags & FDF_MOTOROFF) { 1657 if (bufq_peek(sc->bufq) == NULL) 1658 fdmotoroff(sc); 1659 else { 1660 /* 1661 * we gained a buf request while 1662 * we waited, forget the motoroff 1663 */ 1664 sc->flags &= ~FDF_MOTOROFF; 1665 } 1666 /* 1667 * if we now have DMA unit must have needed 1668 * flushing, quit 1669 */ 1670 if (fdc_indma) { 1671 device_release(sc->sc_dev); 1672 return; 1673 } 1674 } 1675 /* 1676 * if we have no start unit and the current unit has 1677 * io waiting choose this unit to start. 1678 */ 1679 if (ssc == NULL && bufq_peek(sc->bufq) != NULL) 1680 ssc = sc; 1681 else 1682 device_release(sc->sc_dev); 1683 } 1684 if (ssc) { 1685 fdstart(ssc); 1686 device_release(ssc->sc_dev); 1687 } 1688 } 1689 1690 /* 1691 * min byte count to whats left of the track in question 1692 */ 1693 void 1694 fdminphys(struct buf *bp) 1695 { 1696 struct fd_softc *sc; 1697 int sec, toff, tsz; 1698 1699 if ((sc = getsoftc(fd_cd, FDUNIT(bp->b_dev))) == NULL) 1700 panic("fdminphys: couldn't get softc"); 1701 1702 sec = bp->b_blkno % sc->nsectors; 1703 1704 toff = sec * FDSECSIZE; 1705 tsz = sc->nsectors * FDSECSIZE; 1706 #ifdef FDDEBUG 1707 printf("fdminphys: before %ld", bp->b_bcount); 1708 #endif 1709 bp->b_bcount = min(bp->b_bcount, tsz - toff); 1710 #ifdef FDDEBUG 1711 printf(" after %ld\n", bp->b_bcount); 1712 #endif 1713 minphys(bp); 1714 device_release(sc->sc_dev); 1715 } 1716 1717 /* 1718 * encode the track cache into raw MFM ready for DMA 1719 * when we go to multiple disk formats, this will call type dependent 1720 * functions 1721 */ 1722 void fdcachetoraw(struct fd_softc *sc) 1723 { 1724 if (sc->openpart == FDMSDOSPART) 1725 mscachetoraw(sc); 1726 else 1727 amcachetoraw(sc); 1728 } 1729 1730 /* 1731 * decode raw MFM from DMA into units track cache. 1732 * when we go to multiple disk formats, this will call type dependent 1733 * functions 1734 */ 1735 int 1736 fdrawtocache(struct fd_softc *sc) 1737 { 1738 1739 if (sc->openpart == FDMSDOSPART) 1740 return(msrawtocache(sc)); 1741 else 1742 return(amrawtocache(sc)); 1743 } 1744 1745 void 1746 amcachetoraw(struct fd_softc *sc) 1747 { 1748 static u_long mfmnull[4]; 1749 u_long *rp, *crp, *dp, hcksum, dcksum, info; 1750 int sec, i; 1751 1752 rp = fdc_dmap; 1753 1754 /* 1755 * not yet one sector (- 1 long) gap. 1756 * for now use previous drivers values 1757 */ 1758 for (i = 0; i < sc->type->gap; i++) 1759 *rp++ = 0xaaaaaaaa; 1760 /* 1761 * process sectors 1762 */ 1763 dp = sc->cachep; 1764 info = 0xff000000 | (sc->cachetrk << 16) | sc->nsectors; 1765 for (sec = 0; sec < sc->nsectors; sec++, info += (1 << 8) - 1) { 1766 hcksum = dcksum = 0; 1767 /* 1768 * sector format 1769 * offset description 1770 *----------------------------------- 1771 * 0 null 1772 * 1 sync 1773 * oddbits evenbits 1774 *---------------------- 1775 * 2 3 [0xff]b [trk]b [sec]b [togap]b 1776 * 4-7 8-11 null 1777 * 12 13 header cksum [2-11] 1778 * 14 15 data cksum [16-271] 1779 * 16-143 144-271 data 1780 */ 1781 *rp = 0xaaaaaaaa; 1782 if (*(rp - 1) & 0x1) 1783 *rp &= 0x7fffffff; /* clock bit correction */ 1784 rp++; 1785 *rp++ = (FDMFMSYNC << 16) | FDMFMSYNC; 1786 rp = mfmblkencode(&info, rp, &hcksum, 1); 1787 rp = mfmblkencode(mfmnull, rp, &hcksum, 4); 1788 rp = mfmblkencode(&hcksum, rp, NULL, 1); 1789 1790 crp = rp; 1791 rp = mfmblkencode(dp, rp + 2, &dcksum, FDSECLWORDS); 1792 dp += FDSECLWORDS; 1793 crp = mfmblkencode(&dcksum, crp, NULL, 1); 1794 if (*(crp - 1) & 0x1) 1795 *crp &= 0x7fffffff; /* clock bit correction */ 1796 else if ((*crp & 0x40000000) == 0) 1797 *crp |= 0x80000000; 1798 } 1799 *rp = 0xaaa80000; 1800 if (*(rp - 1) & 0x1) 1801 *rp &= 0x7fffffff; 1802 } 1803 1804 u_long * 1805 fdfindsync(u_long *rp, u_long *ep) 1806 { 1807 u_short *sp; 1808 1809 sp = (u_short *)rp; 1810 while ((u_long *)sp < ep && *sp != FDMFMSYNC) 1811 sp++; 1812 while ((u_long *)sp < ep && *sp == FDMFMSYNC) 1813 sp++; 1814 if ((u_long *)sp < ep) 1815 return((u_long *)sp); 1816 return(NULL); 1817 } 1818 1819 int 1820 amrawtocache(struct fd_softc *sc) 1821 { 1822 u_long mfmnull[4]; 1823 u_long *dp, *rp, *erp, *crp, *srp, hcksum, dcksum, info, cktmp; 1824 int cnt, doagain; 1825 1826 doagain = 1; 1827 srp = rp = fdc_dmap; 1828 erp = (u_long *)((u_short *)rp + sc->type->nreadw); 1829 cnt = 0; 1830 again: 1831 if (doagain == 0 || (rp = srp = fdfindsync(srp, erp)) == NULL) { 1832 #ifdef DIAGNOSTIC 1833 aprint_error_dev(sc->sc_dev, "corrupted track (%d) data.\n", 1834 sc->cachetrk); 1835 #endif 1836 return(-1); 1837 } 1838 1839 /* 1840 * process sectors 1841 */ 1842 for (; cnt < sc->nsectors; cnt++) { 1843 hcksum = dcksum = 0; 1844 rp = mfmblkdecode(rp, &info, &hcksum, 1); 1845 rp = mfmblkdecode(rp, mfmnull, &hcksum, 4); 1846 rp = mfmblkdecode(rp, &cktmp, NULL, 1); 1847 if (cktmp != hcksum) { 1848 #ifdef FDDEBUG 1849 printf(" info 0x%lx hchksum 0x%lx trkhcksum 0x%lx\n", 1850 info, hcksum, cktmp); 1851 #endif 1852 goto again; 1853 } 1854 if (((info >> 16) & 0xff) != sc->cachetrk) { 1855 #ifdef DEBUG 1856 aprint_debug_dev(sc->sc_dev, 1857 "incorrect track found: 0x%lx %d\n", 1858 info, sc->cachetrk); 1859 #endif 1860 goto again; 1861 } 1862 #ifdef FDDEBUG 1863 printf(" info 0x%lx\n", info); 1864 #endif 1865 1866 rp = mfmblkdecode(rp, &cktmp, NULL, 1); 1867 dp = sc->cachep; 1868 dp += FDSECLWORDS * ((info >> 8) & 0xff); 1869 crp = mfmblkdecode(rp, dp, &dcksum, FDSECLWORDS); 1870 if (cktmp != dcksum) { 1871 #ifdef FDDEBUG 1872 printf(" info 0x%lx dchksum 0x%lx trkdcksum 0x%lx\n", 1873 info, dcksum, cktmp); 1874 #endif 1875 goto again; 1876 } 1877 1878 /* 1879 * if we are at gap then we can no longer be sure 1880 * of correct sync marks 1881 */ 1882 if ((info & 0xff) == 1) 1883 doagain = 1; 1884 else 1885 doagain = 0; 1886 srp = rp = fdfindsync(crp, erp); 1887 } 1888 return(0); 1889 } 1890 1891 void 1892 mscachetoraw(struct fd_softc *sc) 1893 { 1894 u_short *rp, *erp, crc; 1895 u_char *cp, tb[5]; 1896 int sec, i; 1897 1898 rp = (u_short *)fdc_dmap; 1899 erp = rp + sc->type->nwritew; 1900 cp = sc->cachep; 1901 1902 /* 1903 * initial track filler (828 * GAP1) 1904 */ 1905 for (i = 0; i < sc->type->gap; i++) { 1906 *rp++ = FDMFMGAP1; 1907 *rp++ = FDMFMGAP1; 1908 } 1909 1910 for (sec = 0; sec < sc->nsectors; sec++) { 1911 1912 /* 1913 * leading sector gap 1914 * (12 * GAP2) + (3 * SYNC) 1915 */ 1916 for (i = 0; i < 12; i++) 1917 *rp++ = FDMFMGAP2; 1918 *rp++ = FDMFMSYNC; 1919 *rp++ = FDMFMSYNC; 1920 *rp++ = FDMFMSYNC; 1921 1922 /* 1923 * sector information 1924 * (ID) + track + side + sector + sector size + CRC16 1925 */ 1926 *rp++ = FDMFMID; 1927 tb[0] = sc->cachetrk / FDNHEADS; 1928 tb[1] = sc->cachetrk % FDNHEADS; 1929 tb[2] = sec + 1; 1930 i = sc->bytespersec; 1931 tb[3] = i < 256 ? 0 : (i < 512 ? 1 : (i < 1024 ? 2 : 3)); 1932 rp = msblkencode(rp, tb, 4, &crc); 1933 tb[0] = crc >> 8; 1934 tb[1] = crc & 0xff; 1935 tb[2] = 0x4e; /* GAP1 decoded */ 1936 rp = msblkencode(rp, tb, 3, 0); 1937 1938 /* 1939 * sector info/data gap 1940 * (22 * GAP1) + (12 * GAP2) + (3 * SYNC) 1941 */ 1942 for (i = 0; i < 21; i++) 1943 *rp++ = FDMFMGAP1; 1944 for (i = 0; i < 12; i++) 1945 *rp++ = FDMFMGAP2; 1946 *rp++ = FDMFMSYNC; 1947 *rp++ = FDMFMSYNC; 1948 *rp++ = FDMFMSYNC; 1949 1950 /* 1951 * sector data 1952 * (DATA) + ...data... + CRC16 1953 */ 1954 *rp++ = FDMFMDATA; 1955 rp = msblkencode(rp, cp, sc->bytespersec, &crc); 1956 cp += sc->bytespersec; 1957 tb[0] = crc >> 8; 1958 tb[1] = crc & 0xff; 1959 tb[2] = 0x4e; /* GAP3 decoded */ 1960 rp = msblkencode(rp, tb, 3, 0); 1961 1962 /* 1963 * trailing sector gap 1964 * (80 * GAP3) 1965 */ 1966 for (i = 0; i < 79; i++) 1967 *rp++ = FDMFMGAP3; 1968 } 1969 1970 /* 1971 * fill rest of track with GAP3 1972 */ 1973 while (rp != erp) 1974 *rp++ = FDMFMGAP3; 1975 1976 } 1977 1978 int 1979 msrawtocache(struct fd_softc *sc) 1980 { 1981 u_short *rp, *erp; 1982 u_char tb[5], *cp; 1983 int ct, sec, retry; 1984 1985 rp = (u_short *)fdc_dmap; 1986 erp = rp + sc->type->nreadw; 1987 cp = sc->cachep; 1988 1989 for (ct = 0; ct < sc->nsectors; ct++) { 1990 retry = 1; 1991 do { 1992 /* 1993 * skip leading gap to sync 1994 */ 1995 if ((rp = (u_short *)fdfindsync((u_long *)rp, (u_long *)erp)) == NULL) { 1996 #ifdef DIAGNOSTIC 1997 aprint_normal_dev(sc->sc_dev, 1998 "corrupted track (%d) data.\n", 1999 sc->cachetrk); 2000 #endif 2001 return(-1); 2002 } 2003 2004 /* 2005 * Grab sector info 2006 */ 2007 if (*rp++ != FDMFMID) 2008 continue; 2009 rp = msblkdecode(rp, tb, 4); 2010 #ifdef FDDEBUG 2011 printf("sector id: sector %d, track %d, side %d," 2012 "bps %d\n", tb[2], tb[0], tb[1], 128 << tb[3]); 2013 #endif 2014 if ((tb[0] * FDNHEADS + tb[1]) != sc->cachetrk || 2015 tb[2] > sc->nsectors) 2016 continue; 2017 2018 sec = tb[2]; 2019 sc->bytespersec = 128 << tb[3]; 2020 rp += 2; /* skip CRC-16 */ 2021 2022 /* 2023 * skip gap and read in data 2024 */ 2025 if ((rp = (u_short *)fdfindsync((u_long *)rp, (u_long *)erp)) == NULL) 2026 return(-1); 2027 if (*rp++ != FDMFMDATA) 2028 continue; 2029 rp = msblkdecode(rp, cp + ((sec-1) * sc->bytespersec), 2030 sc->bytespersec); 2031 rp += 2; /* skip CRC-16 */ 2032 2033 retry = 0; 2034 } while (retry); 2035 } 2036 return(0); 2037 } 2038 2039 /* 2040 * encode len longwords of `dp' data in amiga mfm block format (`rp') 2041 * this format specified that the odd bits are at current pos and even 2042 * bits at len + current pos 2043 */ 2044 u_long * 2045 mfmblkencode(u_long *dp, u_long *rp, u_long *cp, int len) 2046 { 2047 u_long *sdp, *edp, d, dtmp, correct; 2048 2049 sdp = dp; 2050 edp = dp + len; 2051 2052 if (*(rp - 1) & 0x1) 2053 correct = 1; 2054 else 2055 correct = 0; 2056 /* 2057 * do odd bits 2058 */ 2059 while (dp < edp) { 2060 d = (*dp >> 1) & 0x55555555; /* remove clock bits */ 2061 dtmp = d ^ 0x55555555; 2062 d |= ((dtmp >> 1) | 0x80000000) & (dtmp << 1); 2063 /* 2064 * correct upper clock bit if needed 2065 */ 2066 if (correct) 2067 d &= 0x7fffffff; 2068 if (d & 0x1) 2069 correct = 1; 2070 else 2071 correct = 0; 2072 /* 2073 * do checksums and store in raw buffer 2074 */ 2075 if (cp) 2076 *cp ^= d; 2077 *rp++ = d; 2078 dp++; 2079 } 2080 /* 2081 * do even bits 2082 */ 2083 dp = sdp; 2084 while (dp < edp) { 2085 d = *dp & 0x55555555; /* remove clock bits */ 2086 dtmp = d ^ 0x55555555; 2087 d |= ((dtmp >> 1) | 0x80000000) & (dtmp << 1); 2088 /* 2089 * correct upper clock bit if needed 2090 */ 2091 if (correct) 2092 d &= 0x7fffffff; 2093 if (d & 0x1) 2094 correct = 1; 2095 else 2096 correct = 0; 2097 /* 2098 * do checksums and store in raw buffer 2099 */ 2100 if (cp) 2101 *cp ^= d; 2102 *rp++ = d; 2103 dp++; 2104 } 2105 if (cp) 2106 *cp &= 0x55555555; 2107 return(rp); 2108 } 2109 2110 /* 2111 * decode len longwords of `dp' data in amiga mfm block format (`rp') 2112 * this format specified that the odd bits are at current pos and even 2113 * bits at len + current pos 2114 */ 2115 u_long * 2116 mfmblkdecode(u_long *rp, u_long *dp, u_long *cp, int len) 2117 { 2118 u_long o, e; 2119 int cnt; 2120 2121 cnt = len; 2122 while (cnt--) { 2123 o = *rp; 2124 e = *(rp + len); 2125 if (cp) { 2126 *cp ^= o; 2127 *cp ^= e; 2128 } 2129 o &= 0x55555555; 2130 e &= 0x55555555; 2131 *dp++ = (o << 1) | e; 2132 rp++; 2133 } 2134 if (cp) 2135 *cp &= 0x55555555; 2136 return(rp + len); 2137 } 2138 2139 /* 2140 * decode len words in standard MFM format to len bytes 2141 * of data. 2142 */ 2143 u_short * 2144 msblkdecode(u_short *rp, u_char *cp, int len) 2145 { 2146 while (len--) { 2147 *cp++ = msdecode[*rp & 0x7f] | 2148 (msdecode[(*rp >> 8) & 0x7f] << 4); 2149 rp++; 2150 } 2151 2152 return(rp); 2153 } 2154 2155 /* 2156 * encode len bytes of data into len words in standard MFM format. 2157 * If a pointer is supplied for crc, calculate the CRC-16 of the data 2158 * as well. 2159 */ 2160 u_short * 2161 msblkencode(u_short *rp, u_char *cp, int len, u_short *crc) 2162 { 2163 u_short td; 2164 u_short mycrc; 2165 2166 /* preload crc for header (4 bytes) 2167 * or data (anything else) 2168 */ 2169 mycrc = (len == 4) ? 0xb230 : 0xe295; 2170 2171 while (len--) { 2172 td = (msencode[*cp >> 4] << 8) | msencode[*cp & 0x0f]; 2173 2174 /* Check for zeros in top bit of encode and bottom 2175 * bit of previous encode. if so, slap a one in betweem 2176 * them. 2177 */ 2178 if ((td & 0x140) == 0) 2179 td |= 0x80; 2180 if ((td & 0x4000) == 0 && (rp[-1] & 1) == 0) 2181 td |= 0x8000; 2182 2183 *rp++ = td; 2184 2185 /* 2186 * calc crc if requested 2187 */ 2188 if (crc) 2189 mycrc = (mycrc << 8) ^ mscrctab[*cp ^ (mycrc >> 8)]; 2190 2191 cp++; 2192 } 2193 2194 if (crc) 2195 *crc = mycrc; 2196 2197 return(rp); 2198 } 2199
Indexes created Thu Oct 02 14:10:14 GMT 2025