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