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