NetBSD/sys/arch/amiga/dev/fd.c

/*	$NetBSD: fd.c,v 1.15 1995/04/06 19:45:00 chopps Exp $	*/

/*
 * Copyright (c) 1994 Christian E. Hopps
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 * 3. All advertising materials mentioning features or use of this software
 *    must display the following acknowledgement:
 *      This product includes software developed by Christian E. Hopps.
 * 4. The name of the author may not be used to endorse or promote products
 *    derived from this software without specific prior written permission
 *
 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
 * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
 * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
 * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
 * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
 * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
#include <sys/param.h>
#include <sys/systm.h>
#include <sys/kernel.h>
#include <sys/malloc.h>
#include <sys/buf.h>
#include <sys/device.h>
#include <sys/ioctl.h>
#include <sys/fcntl.h>
#include <sys/conf.h>
#include <sys/disklabel.h>
#include <sys/disk.h>
#include <sys/dkbad.h>
#include <amiga/amiga/device.h>
#include <amiga/amiga/custom.h>
#include <amiga/amiga/cia.h>
#include <amiga/amiga/cc.h>

enum fdc_bits { FDB_CHANGED = 2, FDB_PROTECT, FDB_CYLZERO, FDB_READY };
/*
 * partitions in fd represent different format floppies
 * partition a is 0 etc..
 */
enum fd_parttypes { 
	FDAMIGAPART = 0, 
#ifdef not_yet
	FDMSDOSPART,
#endif
	FDMAXPARTS
};

#define FDBBSIZE	(8192)
#define FDSBSIZE	(8192)

#define b_cylin	b_resid
#define FDUNIT(dev)	DISKUNIT(dev)
#define FDPART(dev)	DISKPART(dev)
#define FDMAKEDEV(m, u, p)	MAKEDISKDEV((m), (u), (p))

#define FDNHEADS	(2)	/* amiga drives always have 2 heads */
#define FDSECSIZE	(512)	/* amiga drives always have 512 byte sectors */
#define FDSECLWORDS	(128)

#define FDSETTLEDELAY	(18000)	/* usec delay after seeking after switch dir */
#define FDSTEPDELAY	(3500)	/* usec delay after steping */
#define FDPRESIDEDELAY	(1000)	/* usec delay before writing can occur */
#define FDWRITEDELAY	(1300)	/* usec delay after write */

#define FDSTEPOUT	(1)	/* decrease track step */
#define FDSTEPIN	(0)	/* increase track step */

#define FDCUNITMASK	(0x78)	/* mask for all units (bits 6-3) */

#define FDRETRIES	(2)	/* default number of retries */
#define FDMAXUNITS	(4)	/* maximum number of supported units */

#define DISKLEN_READ	(0)	/* fake mask for reading */
#define DISKLEN_WRITE	(1 << 14)	/* bit for writing */
#define DISKLEN_DMAEN	(1 << 15)	/* dma go */
#define DMABUFSZ ((DISKLEN_WRITE - 1) * 2)	/* largest dma possible */

#define FDMFMSYNC	(0x4489)

/*
 * floppy device type
 */
struct fdtype {
	u_int driveid;		/* drive identification (from drive) */
	u_int ncylinders;	/* number of cylinders on drive */
	u_int amiga_nsectors;	/* number of sectors per amiga track */
	u_int msdos_nsectors;	/* number of sectors per msdos track */
	u_int nreadw;		/* number of words (short) read per track */
	u_int nwritew;		/* number of words (short) written per track */
	u_int gap;		/* track gap size in long words */
	u_int precomp[2];	/* 1st and 2nd precomp values */
	char *desc;		/* description of drive type (useq) */
};

/*
 * floppy disk device data
 */
struct fd_softc {
	struct dkdevice dkdev;
	struct buf bufq;	/* queue of buf's */
	struct fdtype *type;
	void *cachep;		/* cached track data (write through) */
	int cachetrk;		/* cahced track -1 for none */
	int hwunit;		/* unit for amiga controlling hw */
	int unitmask;		/* mask for cia select deslect */
	int pstepdir;		/* previous step direction */
	int curcyl;		/* current curcyl head positioned on */
	int flags;		/* misc flags */
	int wlabel;
	int stepdelay;		/* useq to delay after seek user setable */
	int nsectors;		/* number of sectors per track */
	int openpart;		/* which partition [ab] == [12] is open */
	short retries;		/* number of times to retry failed io */
	short retried;		/* number of times current io retried */
};

/* fd_softc->flags */
#define FDF_MOTORON	(0x01)	/* motor is running */
#define FDF_MOTOROFF	(0x02)	/* motor is waiting to be turned off */
#define FDF_WMOTOROFF	(0x04)	/* unit wants a wakeup after off */
#define FDF_DIRTY	(0x08)	/* track cache needs write */
#define FDF_WRITEWAIT	(0x10)	/* need to head select delay on next setpos */
#define FDF_HAVELABEL	(0x20)	/* label is valid */
#define FDF_JUSTFLUSH	(0x40)	/* don't bother caching track. */
#define FDF_NOTRACK0	(0x80)	/* was not able to recalibrate drive */

int fdc_wantwakeup;
int fdc_side;
void  *fdc_dmap;
struct fd_softc *fdc_indma;

struct fdcargs {
	struct fdtype *type;
	int unit;
};

int fdmatch __P((struct device *, struct cfdata *, void *));
int fdcmatch __P((struct device *, struct cfdata *, void *));
int fdcprint __P((void *, char *));
void fdcattach __P((struct device *, struct device *, void *));
void fdattach __P((struct device *, struct device *, void *));

void fdstart __P((struct fd_softc *));
void fddone __P((struct fd_softc *));
void fdfindwork __P((int));
void fddmastart __P((struct fd_softc *, int));
void fddmadone __P((struct fd_softc *, int));
void fdsetpos __P((struct fd_softc *, int, int));
void fdmotoroff __P((void *));
void fdmotorwait __P((void *));
int fdminphys __P((struct buf *));
void fdcachetoraw __P((struct fd_softc *));
int fdrawtocache __P((struct fd_softc *));
int fdloaddisk __P((struct fd_softc *));
u_long *mfmblkencode __P((u_long *, u_long *, u_long *, int));
u_long *mfmblkdecode __P((u_long *, u_long *, u_long *, int));
struct fdtype * fdcgetfdtype __P((int));

void fdstrategy __P((struct buf *));

struct dkdriver fddkdriver = { fdstrategy };

/*
 * read size is (nsectors + 1) * mfm secsize + gap bytes + 2 shorts 
 * write size is nsectors * mfm secsize + gap bytes + 3 shorts
 * the extra shorts are to deal with a dma hw bug in the controller
 * they are probably too much (I belive the bug is 1 short on write and
 * 3 bits on read) but there is no need to be cheap here.
 */
#define MAXTRKSZ (22 * FDSECSIZE)
struct fdtype fdtype[] = {
	{ 0x00000000, 80, 11, 9, 7358, 6815, 414, { 80, 161 }, "3.5dd" },
	{ 0x55555555, 40, 11, 9, 7358, 6815, 414, { 80, 161 }, "5.25dd" },
	{ 0xAAAAAAAA, 80, 22, 18, 14716, 13630, 828, { 80, 161 }, "3.5hd" }
};
int nfdtype = sizeof(fdtype) / sizeof(*fdtype);

struct cfdriver fdcd = {
	NULL, "fd", (cfmatch_t)fdmatch, fdattach, DV_DISK,
	sizeof(struct fd_softc), NULL, 0 };

struct cfdriver fdccd = {
	NULL, "fdc", (cfmatch_t)fdcmatch, fdcattach, DV_DULL,
	sizeof(struct device), NULL, 0 };

/*
 * all hw access through macros, this helps to hide the active low 
 * properties
 */

#define FDUNITMASK(unit)	(1 << (3 + (unit)))

/*
 * select units using mask
 */
#define FDSELECT(um)	do { ciab.prb &= ~(um); } while (0)

/*
 * deselect units using mask
 */
#define FDDESELECT(um)	do { ciab.prb |= (um); delay(1); } while (0)

/*
 * test hw condition bits
 */
#define FDTESTC(bit)	((ciaa.pra & (1 << (bit))) == 0)

/*
 * set motor for select units, true motor on else off
 */
#define FDSETMOTOR(on)	do { \
	if (on) ciab.prb &= ~CIAB_PRB_MTR; else ciab.prb |= CIAB_PRB_MTR; \
	} while (0)

/*
 * set head for select units
 */
#define FDSETHEAD(head)	do { \
	if (head) ciab.prb &= ~CIAB_PRB_SIDE; else ciab.prb |= CIAB_PRB_SIDE; \
	delay(1); } while (0)

/*
 * select direction, true towards spindle else outwards
 */
#define FDSETDIR(in)	do { \
	if (in) ciab.prb &= ~CIAB_PRB_DIR; else ciab.prb |= CIAB_PRB_DIR; \
	delay(1); } while (0)

/*
 * step the selected units
 */
#define FDSTEP	do { \
    ciab.prb &= ~CIAB_PRB_STEP; ciab.prb |= CIAB_PRB_STEP; \
    } while (0)

#define FDDMASTART(len, towrite)	do { \
    int dmasz = (len) | ((towrite) ? DISKLEN_WRITE : 0) | DISKLEN_DMAEN; \
    custom.dsklen = dmasz; custom.dsklen = dmasz; } while (0)

#define FDDMASTOP	do { custom.dsklen = 0; } while (0)


int
fdcmatch(pdp, cfp, auxp)
	struct device *pdp;
	struct cfdata *cfp;
	void *auxp;
{
	if (matchname("fdc", auxp) == 0 || cfp->cf_unit != 0)
		return(0);
	if ((fdc_dmap = alloc_chipmem(DMABUFSZ)) == NULL) {
		printf("fdc: unable to allocate dma buffer\n");
		return(0);
	}
	return(1);
}

void
fdcattach(pdp, dp, auxp)
	struct device *pdp, *dp;
	void *auxp;
{
	struct fdcargs args;

	printf(": dmabuf pa 0x%x\n", kvtop(fdc_dmap));
	args.unit = 0;
	args.type = fdcgetfdtype(args.unit);

	fdc_side = -1;
	config_found(dp, &args, fdcprint);
	for (args.unit++; args.unit < FDMAXUNITS; args.unit++) {
		if ((args.type = fdcgetfdtype(args.unit)) == NULL)
			continue;
		config_found(dp, &args, fdcprint);
	}
}

int
fdcprint(auxp, pnp)
	void *auxp;
	char *pnp;
{
	struct fdcargs *fcp;

	fcp = auxp;
	if (pnp)
		printf("fd%d at %s:", fcp->unit, pnp);
	return(UNCONF);
}

/*ARGSUSED*/
int
fdmatch(pdp, cfp, auxp)
	struct device *pdp;
	struct cfdata *cfp;
	void *auxp;
{
#define cf_unit	cf_loc[0]
	struct fdcargs *fdap;

	fdap = auxp;
	if (cfp->cf_unit == fdap->unit || cfp->cf_unit == -1)
		return(1);
	return(0);
#undef cf_unit
}

void
fdattach(pdp, dp, auxp)
	struct device *pdp, *dp;
	void *auxp;
{
	struct fdcargs *ap;
	struct fd_softc *sc;

	ap = auxp;
	sc = (struct fd_softc *)dp;

	sc->curcyl = sc->cachetrk = -1;
	sc->openpart = -1;
	sc->type = ap->type;
	sc->hwunit = ap->unit;
	sc->unitmask = 1 << (3 + ap->unit);
	sc->retries = FDRETRIES;
	sc->dkdev.dk_driver = &fddkdriver;
	sc->stepdelay = FDSTEPDELAY;
	printf(": %s %d cyl, %d head, %d sec [%d sec], 512 bytes/sec\n",
	    sc->type->desc, sc->type->ncylinders, FDNHEADS,
	    sc->type->amiga_nsectors, sc->type->msdos_nsectors);

	/*
	 * calibrate the drive
	 */
	fdsetpos(sc, 0, 0);
	fdsetpos(sc, sc->type->ncylinders, 0);
	fdsetpos(sc, 0, 0);
	fdmotoroff(sc);

	/*
	 * enable disk related interrupts
	 */
	custom.dmacon = DMAF_SETCLR | DMAF_DISK;
	/* XXX why softint */
	custom.intena = INTF_SETCLR |INTF_SOFTINT | INTF_DSKBLK;
	ciaa.icr = CIA_ICR_IR_SC | CIA_ICR_FLG;
}

/*ARGSUSED*/
int
Fdopen(dev, flags, devtype, p)
	dev_t dev;
	int flags, devtype;
	struct proc *p;
{
	struct fd_softc *sc;
	int wasopen, fwork, error, s;

	error = 0;

	if (FDPART(dev) >= FDMAXPARTS)
		return(ENXIO);

	if ((sc = getsoftc(fdcd, FDUNIT(dev))) == NULL)
		return(ENXIO);
	if (sc->flags & FDF_NOTRACK0)
		return(ENXIO);
	if (sc->cachep == NULL)
		sc->cachep = malloc(MAXTRKSZ, M_DEVBUF, M_WAITOK);

	s = splbio();
	/*
	 * if we are sleeping in Fdclose(); waiting for a chance to
	 * shut the motor off, do a sleep here also.
	 */
	while (sc->flags & FDF_WMOTOROFF)
		tsleep(fdmotoroff, PRIBIO, "Fdopen", 0);

	fwork = 0;
	/*
	 * if not open let user open request type, otherwise 
	 * ensure they are trying to open same type.
	 */
	if (sc->openpart == FDPART(dev))
		wasopen = 1;
	else if (sc->openpart == -1) {
		sc->openpart = FDPART(dev);
		wasopen = 0;
	} else {
		wasopen = 1;
		error = EPERM;
		goto done;
	} 

	/*
	 * wait for current io to complete if any
	 */
	if (fdc_indma) {
		fwork = 1;
		fdc_wantwakeup++;	
		tsleep(Fdopen, PRIBIO, "Fdopen", 0);
	}
	if (error = fdloaddisk(sc))
		goto done;
	if (error = fdgetdisklabel(sc, dev))
		goto done;
#ifdef FDDEBUG
	printf("  open successful\n");
#endif
done:
	/*
	 * if we requested that fddone()->fdfindwork() wake us, allow it to 
	 * complete its job now
	 */
	if (fwork)
		fdfindwork(FDUNIT(dev));
	splx(s);

	/* 
	 * if we were not open and we marked us so reverse that.
	 */
	if (error && wasopen == 0)
		sc->openpart = 0;
	return(error);
}

/*ARGSUSED*/
int
Fdclose(dev, flags, devtype, p)
	dev_t dev;
	int flags, devtype;
	struct proc *p;
{
	struct fd_softc *sc;
	int s;

#ifdef FDDEBUG
	printf("Fdclose()\n");
#endif
	sc = getsoftc(fdcd, FDUNIT(dev));
	s = splbio();
	if (sc->flags & FDF_MOTORON) {
		sc->flags |= FDF_WMOTOROFF;
		tsleep(fdmotoroff, PRIBIO, "Fdclose", 0);
		sc->flags &= ~FDF_WMOTOROFF;
		wakeup(fdmotoroff);
	}
	sc->openpart = -1;
	splx(s);
	return(0);
}

int
fdioctl(dev, cmd, addr, flag, p)
	dev_t dev;
	u_long cmd;
	caddr_t addr;
	int flag;
	struct proc *p;
{
	struct fd_softc *sc;
	void *data;
	int error, wlab;

	sc = getsoftc(fdcd, FDUNIT(dev));
	
	if ((sc->flags & FDF_HAVELABEL) == 0)
		return(EBADF);

	switch (cmd) {
	case DIOCSBAD:
		return(EINVAL);
	case DIOCSRETRIES:
		if (*(int *)addr < 0)
			return(EINVAL);
		sc->retries = *(int *)addr;
		return(0);
	case DIOCSSTEP:
		if (*(int *)addr < FDSTEPDELAY)
			return(EINVAL);
		sc->dkdev.dk_label.d_trkseek = sc->stepdelay = *(int *)addr;
		return(0);
	case DIOCGDINFO:
		*(struct disklabel *)addr = sc->dkdev.dk_label;
		return(0);
	case DIOCGPART:
		((struct partinfo *)addr)->disklab = &sc->dkdev.dk_label;
		((struct partinfo *)addr)->part = 
		    &sc->dkdev.dk_label.d_partitions[FDPART(dev)];
		return(0);
	case DIOCSDINFO:
		if ((flag & FWRITE) == 0)
			return(EBADF);
		return(fdsetdisklabel(sc, (struct disklabel *)addr));
	case DIOCWDINFO:
		if ((flag & FWRITE) == 0)
			return(EBADF);
		if (error = fdsetdisklabel(sc, (struct disklabel *)addr))
			return(error);
		wlab = sc->wlabel;
		sc->wlabel = 1;
		error = fdputdisklabel(sc, dev);
		sc->wlabel = wlab;
		return(error);
	case DIOCWLABEL:
		if ((flag & FWRITE) == 0)
			return(EBADF);
		sc->wlabel = *(int *)addr;
		return(0);
	default:
		return(ENOTTY);
	}
}

/* 
 * no dumps to floppy disks thank you.
 */
int
fdsize(dev)
	dev_t dev;
{
	return(-1);
}

int
fdread(dev, uio)
	dev_t dev;
	struct uio *uio;
{
	return (physio(cdevsw[major(dev)].d_strategy, (struct buf *)NULL,
	    dev, B_READ, fdminphys, uio));
}

int
fdwrite(dev, uio)
	dev_t dev;
	struct uio *uio;
{
	return (physio(cdevsw[major(dev)].d_strategy, (struct buf *)NULL,
	    dev, B_WRITE, fdminphys, uio));
}


int
fdintr()
{
	int s;
	
	s = splbio();
	if (fdc_indma)
		fddmadone(fdc_indma, 0);
	splx(s);
}

void
fdstrategy(bp)
	struct buf *bp;
{
	struct disklabel *lp;
	struct fd_softc *sc;
	struct buf *dp;
	int unit, part, s;

	unit = FDUNIT(bp->b_dev);
	part = FDPART(bp->b_dev);
	sc = getsoftc(fdcd, unit);

#ifdef FDDEBUG
	printf("fdstrategy: 0x%x\n", bp);
#endif
	/*
	 * check for valid partition and bounds
	 */
	lp = &sc->dkdev.dk_label;
	if ((sc->flags & FDF_HAVELABEL) == 0) {
		bp->b_error = EIO;
		goto bad;
	}		
	if (bounds_check_with_label(bp, lp, sc->wlabel) <= 0)
		goto done;

	/*
	 * trans count of zero or bounds check indicates io is done
	 * we are done.
	 */
	if (bp->b_bcount == 0)
		goto done;

	/*
	 * queue the buf and kick the low level code
	 */
	s = splbio();
	dp = &sc->bufq;
	disksort(dp, bp);
	fdstart(sc);
	splx(s);
	return;
bad:
	bp->b_flags |= B_ERROR;
done:
	bp->b_resid = bp->b_bcount;
	biodone(bp);
}

/*
 * make sure disk is loaded and label is up-to-date.
 */
int
fdloaddisk(sc)
	struct fd_softc *sc;
{
	/*
	 * if diskchange is low step drive to 0 then up one then to zero.
	 */
	fdsetpos(sc, 0, 0);
	if (FDTESTC(FDB_CHANGED)) {
		sc->cachetrk = -1;		/* invalidate the cache */
		sc->flags &= ~FDF_HAVELABEL;
		fdsetpos(sc, FDNHEADS, 0);
		fdsetpos(sc, 0, 0);
		if (FDTESTC(FDB_CHANGED)) {
			fdmotoroff(sc);
			return(ENXIO);
		}
	}
	fdmotoroff(sc);
	sc->type = fdcgetfdtype(sc->hwunit);
	if (sc->type == NULL)
		return(ENXIO);
#ifdef not_yet
	if (sc->openpart == FDMSDOSPART)
		sc->nsectors = sc->type->msdos_nsectors;
	else
#endif
		sc->nsectors = sc->type->amiga_nsectors;
	return(0);
}

/*
 * read disk label, if present otherwise create one
 * return a new label if raw part and none found, otherwise err.
 */
int
fdgetdisklabel(sc, dev)
	struct fd_softc *sc;
	dev_t dev;
{
	struct disklabel *lp, *dlp;
	struct cpu_disklabel *clp;
	struct buf *bp;
	int error, part;

	if (sc->flags & FDF_HAVELABEL)
		return(0);
#ifdef FDDEBUG
	printf("fdgetdisklabel()\n");
#endif
	part = FDPART(dev);
	lp = &sc->dkdev.dk_label;
	clp =  &sc->dkdev.dk_cpulabel;
	bzero(lp, sizeof(struct disklabel));
	bzero(clp, sizeof(struct cpu_disklabel));
	
	lp->d_secsize = FDSECSIZE;
	lp->d_ntracks = FDNHEADS;
	lp->d_ncylinders = sc->type->ncylinders;
	lp->d_nsectors = sc->nsectors;
	lp->d_secpercyl = lp->d_ntracks * lp->d_nsectors;
	lp->d_secperunit = lp->d_secpercyl * lp->d_ncylinders;
	lp->d_npartitions = part + 1;
	lp->d_partitions[part].p_size = lp->d_secperunit;
	lp->d_partitions[part].p_fstype = FS_UNUSED;
	lp->d_partitions[part].p_fsize = 1024;
	lp->d_partitions[part].p_frag = 8;
	
	sc->flags |= FDF_HAVELABEL;
	
	bp = (void *)geteblk((int)lp->d_secsize);
	bp->b_dev = dev; 
	bp->b_blkno = 0;
	bp->b_cylin = 0;
	bp->b_bcount = FDSECSIZE;
	bp->b_flags = B_BUSY | B_READ;
	fdstrategy(bp);
	if (error = biowait(bp))
		goto nolabel;
	dlp = (struct disklabel *)(bp->b_data + LABELOFFSET);
	if (dlp->d_magic != DISKMAGIC || dlp->d_magic2 != DISKMAGIC ||
	    dkcksum(dlp)) {
		error = EINVAL;
		goto nolabel;
	}
	bcopy(dlp, lp, sizeof(struct disklabel));
	if (lp->d_trkseek > FDSTEPDELAY)
		sc->stepdelay = lp->d_trkseek;
	brelse(bp);
	return(0);
nolabel:
	bzero(lp, sizeof(struct disklabel));
	lp->d_secsize = FDSECSIZE;
	lp->d_ntracks = FDNHEADS;
	lp->d_ncylinders = sc->type->ncylinders;
	lp->d_nsectors = sc->nsectors;
	lp->d_secpercyl = lp->d_ntracks * lp->d_nsectors;
	lp->d_type = DTYPE_FLOPPY;
	lp->d_secperunit = lp->d_secpercyl * lp->d_ncylinders;
	lp->d_rpm = 300; 		/* good guess I suppose. */
	lp->d_interleave = 1;		/* should change when adding msdos */
	sc->stepdelay = lp->d_trkseek = FDSTEPDELAY;	
	lp->d_bbsize = 0;
	lp->d_sbsize = 0;
	lp->d_partitions[part].p_size = lp->d_secperunit;
	lp->d_partitions[part].p_fstype = FS_UNUSED;
	lp->d_partitions[part].p_fsize = 1024;
	lp->d_partitions[part].p_frag = 8;
	lp->d_npartitions = part + 1;
	lp->d_magic = lp->d_magic2 = DISKMAGIC;
	lp->d_checksum = dkcksum(lp);
	brelse(bp);
	return(0);
}

/*
 * set the incore copy of this units disklabel
 */
int
fdsetdisklabel(sc, lp)
	struct fd_softc *sc;
	struct disklabel *lp;
{
	struct disklabel *clp;
	struct partition *pp;

	/*
	 * must have at least opened raw unit to fetch the 
	 * raw_part stuff.
	 */
	if ((sc->flags & FDF_HAVELABEL) == 0)
		return(EINVAL);
	clp = &sc->dkdev.dk_label;
	/*
	 * make sure things check out and we only have one valid
	 * partition
	 */
#ifdef FDDEBUG
	printf("fdsetdisklabel\n");
#endif
	if (lp->d_secsize != FDSECSIZE ||
	    lp->d_nsectors != clp->d_nsectors ||
	    lp->d_ntracks != FDNHEADS ||
	    lp->d_ncylinders != clp->d_ncylinders ||
	    lp->d_secpercyl != clp->d_secpercyl ||
	    lp->d_secperunit != clp->d_secperunit ||
	    lp->d_magic != DISKMAGIC ||
	    lp->d_magic2 != DISKMAGIC ||
	    lp->d_npartitions == 0 ||
	    lp->d_npartitions > FDMAXPARTS ||
	    (lp->d_partitions[0].p_offset && lp->d_partitions[1].p_offset) ||
	    dkcksum(lp))
		return(EINVAL);
	/* 
	 * if any partitions are present make sure they 
	 * represent the currently open type
	 */
	if ((pp = &lp->d_partitions[0])->p_size) {
		if ((pp = &lp->d_partitions[1])->p_size == 0)
			goto done;
		else if (sc->openpart != 1)
			return(EINVAL);
	} else if (sc->openpart != 0)
		return(EINVAL);
	/* 
	 * make sure selected partition is within bounds
	 * XXX on the second check, its to handle a bug in 
	 * XXX the cluster routines as they require mutliples
	 * XXX of CLBYTES currently
	 */
	if ((pp->p_offset + pp->p_size >= lp->d_secperunit) ||
	    (pp->p_frag * pp->p_fsize % CLBYTES))
		return(EINVAL);
done:
	bcopy(lp, clp, sizeof(struct disklabel));
	return(0);
}

/*
 * write out the incore copy of this units disklabel
 */
int
fdputdisklabel(sc, dev)
	struct fd_softc *sc;
	dev_t dev;
{
	struct disklabel *lp, *dlp;
	struct buf *bp;
	int error;
	
	if ((sc->flags & FDF_HAVELABEL) == 0)
		return(EBADF);
#ifdef FDDEBUG
	printf("fdputdisklabel\n");
#endif
	/*
	 * get buf and read in sector 0
	 */
	lp = &sc->dkdev.dk_label;
	bp = (void *)geteblk((int)lp->d_secsize);
	bp->b_dev = FDMAKEDEV(major(dev), FDUNIT(dev), RAW_PART);
	bp->b_blkno = 0;
	bp->b_cylin = 0;
	bp->b_bcount = FDSECSIZE;
	bp->b_flags = B_BUSY | B_READ;
	fdstrategy(bp);
	if (error = biowait(bp))
		goto done;
	/*
	 * copy disklabel to buf and write it out syncronous
	 */
	dlp = (struct disklabel *)(bp->b_data + LABELOFFSET);
	bcopy(lp, dlp, sizeof(struct disklabel));
	bp->b_blkno = 0;
	bp->b_cylin = 0;
	bp->b_flags = B_WRITE;
	fdstrategy(bp);
	error = biowait(bp);
done:
	brelse(bp);
	return(error);
}

/*
 * figure out drive type or NULL if none.
 */
struct fdtype *
fdcgetfdtype(unit)
	int unit;
{
	struct fdtype *ftp;
	u_long id, idb;
	int cnt, umask;

	id = 0;
	umask = 1 << (3 + unit);

	FDDESELECT(FDCUNITMASK);

	FDSETMOTOR(1);
	delay(1);
	FDSELECT(umask);
	delay(1);
	FDDESELECT(umask);

	FDSETMOTOR(0);
	delay(1);
	FDSELECT(umask);
	delay(1);
	FDDESELECT(umask);

	for (idb = 0x80000000; idb; idb >>= 1) {
		FDSELECT(umask);
		delay(1);
		if (FDTESTC(FDB_READY) == 0)
			id |= idb;
		FDDESELECT(umask);
		delay(1);
	}
#ifdef FDDEBUG
	printf("fdcgettype unit %d id 0x%x\n", unit, id);
#endif

	for (cnt = 0, ftp = fdtype; cnt < nfdtype; ftp++, cnt++)
		if (ftp->driveid == id)
			return(ftp);
	/*
	 * 3.5dd's at unit 0 do not always return id.
	 */
	if (unit == 0)
		return(fdtype);
	return(NULL);
}

/*
 * turn motor off if possible otherwise mark as needed and will be done
 * later.
 */
void
fdmotoroff(arg)
	void *arg;
{
	struct fd_softc *sc;
	int unitmask, s;

	sc = arg;
	s = splbio();

#ifdef FDDEBUG
	printf("fdmotoroff: unit %d\n", sc->hwunit);
#endif
	if ((sc->flags & FDF_MOTORON) == 0) 
		goto done;
	/*
	 * if we have a timeout on a dma operation let fddmadone()
	 * deal with it.
	 */
	if (fdc_indma == sc) {
		fddmadone(sc, 1);
		goto done;
	}
#ifdef FDDEBUG
	printf(" motor was on, turning off\n");
#endif

	/*
	 * flush cache if needed
	 */
	if (sc->flags & FDF_DIRTY) {
		sc->flags |= FDF_JUSTFLUSH | FDF_MOTOROFF;
#ifdef FDDEBUG
		printf("  flushing dirty buffer first\n");
#endif
		/*
		 * if dma'ing done for now, fddone() will call us again
		 */
		if (fdc_indma)
			goto done;
		fddmastart(sc, sc->cachetrk);
		goto done;
	}

	/*
	 * if controller is busy just schedule us to be called back
	 */
	if (fdc_indma) {
		/*
		 * someone else has the controller now
		 * just set flag and let fddone() call us again.
		 */
		sc->flags |= FDF_MOTOROFF;
		goto done;
	}

#ifdef FDDEBUG
	printf("  hw turing unit off\n");
#endif

	sc->flags &= ~(FDF_MOTORON | FDF_MOTOROFF);
	FDDESELECT(FDCUNITMASK);
	FDSETMOTOR(0);
	delay(1);
	FDSELECT(sc->unitmask);
	delay(4);
	FDDESELECT(sc->unitmask);
	delay(1);
	if (sc->flags & FDF_WMOTOROFF)
		wakeup(fdmotoroff);
done:
	splx(s);
}

/*
 * select drive seek to track exit with motor on.
 * fdsetpos(x, 0, 0) does calibrates the drive.
 */
void
fdsetpos(sc, trk, towrite)
	struct fd_softc *sc;
	int trk, towrite;
{
	int nstep, sdir, ondly, ncyl, nside;

	FDDESELECT(FDCUNITMASK);
	FDSETMOTOR(1);
	delay(1);
	FDSELECT(sc->unitmask);
	delay(1);
	if ((sc->flags & FDF_MOTORON) == 0) {
		ondly = 0;
		while (FDTESTC(FDB_READY) == 0) {
			delay(1000);
			if (++ondly >= 1000)
				break;
		}
	}
	sc->flags |= FDF_MOTORON;

	ncyl = trk / FDNHEADS;
	nside = trk % FDNHEADS;

	if (sc->curcyl == ncyl && fdc_side == nside)
		return;

	if (towrite)
		sc->flags |= FDF_WRITEWAIT;
	
#ifdef FDDEBUG
	printf("fdsetpos: cyl %d head %d towrite %d\n", trk / FDNHEADS, 
	    trk % FDNHEADS, towrite);
#endif
	nstep = ncyl - sc->curcyl;
	if (nstep) {
		/*
		 * figure direction
		 */
		if (nstep > 0 && ncyl != 0) {
			sdir = FDSTEPIN;
			FDSETDIR(1);
		} else {
			nstep = -nstep;
			sdir = FDSTEPOUT;
			FDSETDIR(0);
		}
		if (ncyl == 0) {
			/*
			 * either just want cylinder 0 or doing 
			 * a calibrate.
			 */
			nstep = 256;
			while (FDTESTC(FDB_CYLZERO) == 0 && nstep--) {
				FDSTEP;
				delay(sc->stepdelay);
			}
			if (nstep < 0)
				sc->flags |= FDF_NOTRACK0;
		} else {
			/*
			 * step the needed amount amount.
			 */
			while (nstep--) {
				FDSTEP;
				delay(sc->stepdelay);
			}
		}
		/*
		 * if switched directions 
		 * allow drive to settle.
		 */
		if (sc->pstepdir != sdir)
			delay(FDSETTLEDELAY);
		sc->pstepdir = sdir;
		sc->curcyl = ncyl;
	}
	if (nside == fdc_side)
		return;
	/*
	 * select side
	 */
	fdc_side = nside;
	FDSETHEAD(nside);
	delay(FDPRESIDEDELAY);
}

void
fdselunit(sc)
	struct fd_softc *sc;
{
	FDDESELECT(FDCUNITMASK);		/* deselect all */
	FDSETMOTOR(sc->flags & FDF_MOTORON);	/* set motor to unit's state */
	delay(1);
	FDSELECT(sc->unitmask);			/* select unit */
	delay(1);
}

/*
 * process next buf on device queue.
 * normall sequence of events:
 * fdstart() -> fddmastart(); 
 * fdintr() -> fddmadone() -> fddone();
 * if the track is in the cache then fdstart() will short-circuit
 * to fddone() else if the track cache is dirty it will flush.  If
 * the buf is not an entire track it will cache the requested track.
 */
void
fdstart(sc)
	struct fd_softc *sc;
{
	int trk, error, write;
	struct buf *bp, *dp;

#ifdef FDDEBUG
	printf("fdstart: unit %d\n", sc->hwunit);
#endif

	/*
	 * if dma'ing just return. we must have been called from fdstartegy.
	 */
	if (fdc_indma)
		return;

	/*
	 * get next buf if there.
	 */
	dp = &sc->bufq;
	if ((bp = dp->b_actf) == NULL) {
#ifdef FDDEBUG
		printf("  nothing to do\n");
#endif
		return;
	}
	
	/*
	 * make sure same disk is loaded
	 */
	fdselunit(sc);
	if (FDTESTC(FDB_CHANGED)) {
		/*
		 * disk missing, invalidate all future io on 
		 * this unit until re-open()'ed also invalidate 
		 * all current io
		 */
#ifdef FDDEBUG
		printf("  disk was removed invalidating all io\n");
#endif
		sc->flags &= ~FDF_HAVELABEL;
		for (;;) {
			bp->b_flags |= B_ERROR;
			bp->b_error = EIO;
			if (bp->b_actf == NULL)
				break;
			biodone(bp);
			bp = bp->b_actf;
		}
		/*
		 * do fddone() on last buf to allow other units to start.
		 */
		dp->b_actf = bp;
		fddone(sc);
		return;
	}

	/* 
	 * we have a valid buf, setup our local version
	 * we use this count to allow reading over multiple tracks.
	 * into a single buffer
	 */
	dp->b_bcount = bp->b_bcount;
	dp->b_blkno = bp->b_blkno;
	dp->b_data = bp->b_data;
	dp->b_flags = bp->b_flags;
	dp->b_resid = 0;
	
	if (bp->b_flags & B_READ)
		write = 0;
	else if (FDTESTC(FDB_PROTECT) == 0)
		write = 1;
	else {
		error = EPERM;
		goto bad;
	}

	/*
	 * figure trk given blkno
	 */
	trk = bp->b_blkno / sc->nsectors;
	
	/*
	 * check to see if same as currently cached track
	 * if so we need to do no dma read.
	 */
	if (trk == sc->cachetrk) {
		fddone(sc);
		return;
	}
		
	/*
	 * if we will be overwriting the entire cache, don't bother to 
	 * fetch it.
	 */
	if (bp->b_bcount == (sc->nsectors * FDSECSIZE) && write &&
	    bp->b_blkno % sc->nsectors == 0) {
		if (sc->flags & FDF_DIRTY)
			sc->flags |= FDF_JUSTFLUSH;
		else {
			sc->cachetrk = trk;
			fddone(sc);
			return;
		}
	}
	
	/*
	 * start dma read of `trk'
	 */
	fddmastart(sc, trk);
	return;
bad:
	bp->b_flags |= B_ERROR;
	bp->b_error = error;
	fddone(sc);
}

/*
 * continue a started operation on next track. always begin at 
 * sector 0 on the next track.
 */
void
fdcont(sc)
	struct fd_softc *sc;
{
	struct buf *dp, *bp;
	int trk, write;
	
	dp = &sc->bufq;
	bp = dp->b_actf;
	dp->b_data += (dp->b_bcount - bp->b_resid);
	dp->b_blkno += (dp->b_bcount - bp->b_resid) / FDSECSIZE;
	dp->b_bcount = bp->b_resid;

	/*
	 * figure trk given blkno
	 */
	trk = dp->b_blkno / sc->nsectors;
#ifdef DEBUG
	if (trk != sc->cachetrk + 1 || dp->b_blkno % sc->nsectors != 0)
		panic("fdcont: confused");
#endif
	if (dp->b_flags & B_READ)
		write = 0;
	else
		write = 1;
	/*
	 * if we will be overwriting the entire cache, don't bother to 
	 * fetch it.
	 */
	if (dp->b_bcount == (sc->nsectors * FDSECSIZE) && write) {
		if (sc->flags & FDF_DIRTY)
			sc->flags |= FDF_JUSTFLUSH;
		else {
			sc->cachetrk = trk;
			fddone(sc);
			return;
		}
	}
	/*
	 * start dma read of `trk'
	 */
	fddmastart(sc, trk);
	return;
}

void
fddmastart(sc, trk)
	struct fd_softc *sc;
	int trk;
{
	int adkmask, ndmaw, write, dmatrk;

#ifdef FDDEBUG
	printf("fddmastart: unit %d cyl %d head %d", sc->hwunit, 
	    trk / FDNHEADS, trk % FDNHEADS);
#endif
	/*
	 * flush the cached track if dirty else read requested track.
	 */
	if (sc->flags & FDF_DIRTY) {
		fdcachetoraw(sc);
		ndmaw = sc->type->nwritew;
		dmatrk = sc->cachetrk;
		write = 1;
	} else {
		ndmaw = sc->type->nreadw;
		dmatrk = trk;
		write = 0;
	}

#ifdef FDDEBUG
	printf(" %s", write ? " flushing cache\n" : " loading cache\n");
#endif
	sc->cachetrk = trk;
	fdc_indma = sc;
	fdsetpos(sc, dmatrk, write);

	/*
	 * setup dma stuff
	 */
	if (write == 0) {
		custom.adkcon = ADKF_MSBSYNC;
		custom.adkcon = ADKF_SETCLR | ADKF_WORDSYNC | ADKF_FAST;
		custom.dsksync = FDMFMSYNC;
	} else {
		custom.adkcon = ADKF_PRECOMP1 | ADKF_PRECOMP0 | ADKF_WORDSYNC |
		    ADKF_MSBSYNC;
		adkmask = ADKF_SETCLR | ADKF_FAST | ADKF_MFMPREC;
		if (dmatrk >= sc->type->precomp[0])
			adkmask |= ADKF_PRECOMP0;
		if (dmatrk >= sc->type->precomp[1])
			adkmask |= ADKF_PRECOMP1;
		custom.adkcon = adkmask;
	}
	custom.dskpt = (u_char *)kvtop(fdc_dmap);
	FDDMASTART(ndmaw, write);

#ifdef FDDEBUG
	printf("  dma started\n");
#endif
}

/*
 * recalibrate the drive
 */
void
fdcalibrate(arg)
	void *arg;
{
	struct fd_softc *sc;
	static int loopcnt;

	sc = arg;

	if (loopcnt == 0) {
		/*
		 * seek cyl 0
		 */
		fdc_indma = sc;
		sc->stepdelay += 900;
		if (sc->cachetrk > 1)
			fdsetpos(sc, sc->cachetrk % FDNHEADS, 0);
		sc->stepdelay -= 900;
	}
	if (loopcnt++ & 1)
		fdsetpos(sc, sc->cachetrk, 0);
	else
		fdsetpos(sc, sc->cachetrk + FDNHEADS, 0);
	/*
	 * trk++, trk, trk++, trk, trk++, trk, trk++, trk and dma
	 */
	if (loopcnt < 8)
		timeout(fdcalibrate, sc, hz / 8);
	else {
		loopcnt = 0;
		fdc_indma = NULL;
		timeout(fdmotoroff, sc, 3 * hz / 2);
		fddmastart(sc, sc->cachetrk);
	}
}

void
fddmadone(sc, timeo)
	struct fd_softc *sc;
	int timeo;
{
#ifdef FDDEBUG
	printf("fddmadone: unit %d, timeo %d\n", sc->hwunit, timeo);
#endif
	fdc_indma = NULL;
	untimeout(fdmotoroff, sc);
	FDDMASTOP;

	/*
	 * guarantee the drive has been at current head and cyl
	 * for at least FDWRITEDELAY after a write.
	 */
	if (sc->flags & FDF_WRITEWAIT) {
		delay(FDWRITEDELAY);
		sc->flags &= ~FDF_WRITEWAIT;
	}

	if ((sc->flags & FDF_MOTOROFF) == 0) {
		/*
		 * motor runs for 1.5 seconds after last dma
		 */
		timeout(fdmotoroff, sc, 3 * hz / 2);
	}
	if (sc->flags & FDF_DIRTY) {
		/*
		 * if buffer dirty, the last dma cleaned it
		 */
		sc->flags &= ~FDF_DIRTY;
		if (timeo)
			printf("%s: write of track cache timed out.\n",
			    sc->dkdev.dk_dev.dv_xname);
		if (sc->flags & FDF_JUSTFLUSH) {
			sc->flags &= ~FDF_JUSTFLUSH;
			/*
			 * we are done dma'ing
			 */
			fddone(sc);
			return;
		}
		/*
		 * load the cache
		 */
		fddmastart(sc, sc->cachetrk);
		return;
	}
#ifdef FDDEBUG
	else if (sc->flags & FDF_MOTOROFF)
		panic("fddmadone: FDF_MOTOROFF with no FDF_DIRTY");
#endif

	/*
	 * cache loaded decode it into cache buffer
	 */
	if (timeo == 0 && fdrawtocache(sc) == 0)
		sc->retried = 0;
	else {
#ifdef FDDEBUG
		if (timeo)
			printf("%s: fddmadone: cache load timed out.\n",
			    sc->dkdev.dk_dev.dv_xname);
#endif
		if (sc->retried >= sc->retries) {
			sc->retried = 0;
			sc->cachetrk = -1;
		} else {
			sc->retried++;
			/*
			 * this will be restarted at end of calibrate loop.
			 */
			untimeout(fdmotoroff, sc);
			fdcalibrate(sc);
			return;
		}
	}
	fddone(sc);
}

void
fddone(sc)
	struct fd_softc *sc;
{
	struct buf *dp, *bp;
	char *data;
	int sz, blk;

#ifdef FDDEBUG
	printf("fddone: unit %d\n", sc->hwunit);
#endif
	/*
	 * check to see if unit is just flushing the cache,
	 * that is we have no io queued.
	 */
	if (sc->flags & FDF_MOTOROFF)
		goto nobuf;

	dp = &sc->bufq;
	if ((bp = dp->b_actf) == NULL)
		panic ("fddone");
	/*
	 * check for an error that may have occured
	 * while getting the track.
	 */
	if (sc->cachetrk == -1) {
		sc->retried = 0;
		bp->b_flags |= B_ERROR;
		bp->b_error = EIO;
	} else if ((bp->b_flags & B_ERROR) == 0) {
		data = sc->cachep;
		/*
		 * get offset of data in track cache and limit
		 * the copy size to not exceed the cache's end.
		 */
		data += (dp->b_blkno % sc->nsectors) * FDSECSIZE;
		sz = sc->nsectors - dp->b_blkno % sc->nsectors;
		sz *= FDSECSIZE;
		sz = min(dp->b_bcount, sz);
		if (bp->b_flags & B_READ)
			bcopy(data, dp->b_data, sz);
		else {
			bcopy(dp->b_data, data, sz);
			sc->flags |= FDF_DIRTY;
		}
		bp->b_resid = dp->b_bcount - sz;
		if (bp->b_resid == 0) {
			bp->b_error = 0;
		} else {
			/*
			 * not done yet need to read next track
			 */
			fdcont(sc);
			return;
		}
	}
	/*
	 * remove from queue.
	 */
	dp->b_actf = bp->b_actf;
	biodone(bp);
nobuf:
	fdfindwork(sc->dkdev.dk_dev.dv_unit);
}

void
fdfindwork(unit)
	int unit;
{ 
	struct fd_softc *ssc, *sc;
	int i, last;

	/*
	 * first see if we have any Fdopen()'s waiting
	 */
	if (fdc_wantwakeup) {
		wakeup(Fdopen);
		fdc_wantwakeup--;
		return;
	}

	/*
	 * start next available unit, linear search from the next unit
	 * wrapping and finally this unit.
	 */
	last = 0;
	ssc = NULL;
	for (i = unit + 1; last == 0; i++) {
		if (i == unit)
			last = 1;
		if (i >= fdcd.cd_ndevs) {
			i = -1;
			continue;
		}
		if ((sc = fdcd.cd_devs[i]) == NULL)
			continue;

		/*
		 * if unit has requested to be turned off 
		 * and it has no buf's queued do it now
		 */
		if (sc->flags & FDF_MOTOROFF) {
			if (sc->bufq.b_actf == NULL)
				fdmotoroff(sc);
			else {
				/*
				 * we gained a buf request while 
				 * we waited, forget the motoroff
				 */
				sc->flags &= ~FDF_MOTOROFF;
			}
			/*
			 * if we now have dma unit must have needed
			 * flushing, quit
			 */
			if (fdc_indma)
				return;
		} 
		/*
		 * if we have no start unit and the current unit has
		 * io waiting choose this unit to start.
		 */
		if (ssc == NULL && sc->bufq.b_actf)
			ssc = sc;
	}
	if (ssc)
		fdstart(ssc);
}

/*
 * min byte count to whats left of the track in question
 */
int
fdminphys(bp)
	struct buf *bp;
{
	struct fd_softc *sc;
	int trk, sec, toff, tsz;

	if ((sc = getsoftc(fdcd, FDUNIT(bp->b_dev))) == NULL)
		return(ENXIO);

	trk = bp->b_blkno / sc->nsectors;
	sec = bp->b_blkno % sc->nsectors;

	toff = sec * FDSECSIZE;
	tsz = sc->nsectors * FDSECSIZE;
#ifdef FDDEBUG
	printf("fdminphys: before %d", bp->b_bcount);
#endif
	bp->b_bcount = min(bp->b_bcount, tsz - toff);
#ifdef FDDEBUG
	printf(" after %d\n", bp->b_bcount);
#endif
	return(bp->b_bcount);
}

/*
 * encode the track cache into raw MFM ready for dma
 * when we go to multiple disk formats, this will call type dependent
 * functions
 */
void
fdcachetoraw(sc)
	struct fd_softc *sc;
{
	static u_long mfmnull[4];
	u_long *rp, *crp, *dp, hcksum, dcksum, info, zero;
	int sec, i;

	rp = fdc_dmap;

	/*
	 * not yet one sector (- 1 long) gap.
	 * for now use previous drivers values
	 */
	for (i = 0; i < sc->type->gap; i++)
		*rp++ = 0xaaaaaaaa;
	/*
	 * process sectors
	 */
	dp = sc->cachep;
	zero = 0;
	info = 0xff000000 | (sc->cachetrk << 16) | sc->nsectors;
	for (sec = 0; sec < sc->nsectors; sec++, info += (1 << 8) - 1) {
		hcksum = dcksum = 0;
		/*
		 * sector format
		 *	offset		description
		 *-----------------------------------
		 *  0			null 
		 *  1			sync 
		 * oddbits	evenbits
		 *----------------------
		 *  2		3	[0xff]b [trk]b [sec]b [togap]b
		 *  4-7		8-11	null
		 * 12		13	header cksum [2-11]
		 * 14		15	data cksum [16-271]
		 * 16-143	144-271	data
		 */
		*rp = 0xaaaaaaaa;
		if (*(rp - 1) & 0x1)
			*rp &= 0x7fffffff;	/* clock bit correction */
		rp++;
		*rp++ = (FDMFMSYNC << 16) | FDMFMSYNC;
		rp = mfmblkencode(&info, rp, &hcksum, 1);
		rp = mfmblkencode(mfmnull, rp, &hcksum, 4);
		rp = mfmblkencode(&hcksum, rp, NULL, 1);

		crp = rp;
		rp = mfmblkencode(dp, rp + 2, &dcksum, FDSECLWORDS);
		dp += FDSECLWORDS;
		crp = mfmblkencode(&dcksum, crp, NULL, 1);
		if (*(crp - 1) & 0x1)
			*crp &= 0x7fffffff;	/* clock bit correction */
		else if ((*crp & 0x40000000) == 0)
			*crp |= 0x80000000;
        }
	*rp = 0xaaa80000;
	if (*(rp - 1) & 0x1)
		*rp &= 0x7fffffff;
}

u_long *
fdfindsync(rp, ep)
	u_long *rp, *ep;
{
	u_short *sp;

	sp = (u_short *)rp;
	while ((u_long *)sp < ep && *sp != FDMFMSYNC)
		sp++;
	while ((u_long *)sp < ep && *sp == FDMFMSYNC)
		sp++;
	if ((u_long *)sp < ep)
		return((u_long *)sp);
	return(NULL);
}

/*
 * decode raw MFM from dma into units track cache.
 * when we go to multiple disk formats, this will call type dependent
 * functions
 */
int
fdrawtocache(sc)
	struct fd_softc *sc;
{
	u_long mfmnull[4];
	u_long *dp, *rp, *erp, *crp, *srp, hcksum, dcksum, info, cktmp;
	int cnt, doagain;

	doagain = 1;
	srp = rp = fdc_dmap;
	erp = (u_long *)((u_short *)rp + sc->type->nreadw);
	cnt = 0;
again:
	if (doagain == 0 || (rp = srp = fdfindsync(srp, erp)) == NULL) {
#ifdef DIAGNOSTIC
		printf("%s: corrupted track (%d) data.\n",
		    sc->dkdev.dk_dev.dv_xname, sc->cachetrk);
#endif
		return(-1);
	}
	
	/*
	 * process sectors
	 */
	for (; cnt < sc->nsectors; cnt++) {
		hcksum = dcksum = 0;
		rp = mfmblkdecode(rp, &info, &hcksum, 1);
		rp = mfmblkdecode(rp, mfmnull, &hcksum, 4);
		rp = mfmblkdecode(rp, &cktmp, NULL, 1);
		if (cktmp != hcksum) {
#ifdef FDDEBUG
			printf("  info 0x%x hchksum 0x%x trkhcksum 0x%x\n",
			    info, hcksum, cktmp);
#endif
			goto again;
		}
		if (((info >> 16) & 0xff) != sc->cachetrk) {
#ifdef DEBUG
			printf("%s: incorrect track found: 0x%0x %d\n",
			    sc->dkdev.dk_dev.dv_xname, info, sc->cachetrk);
#endif
			goto again;
		}
#ifdef FDDEBUG
		printf("  info 0x%x\n", info);
#endif

		rp = mfmblkdecode(rp, &cktmp, NULL, 1);
		dp = sc->cachep;
		dp += FDSECLWORDS * ((info >> 8) & 0xff);
		crp = mfmblkdecode(rp, dp, &dcksum, FDSECLWORDS);
		if (cktmp != dcksum) {
#ifdef FDDEBUG
			printf("  info 0x%x dchksum 0x%x trkdcksum 0x%x\n",
			    info, dcksum, cktmp);
#endif
			goto again;
		}

		/*
		 * if we are at gap then we can no longer be sure
		 * of correct sync marks
		 */
		if ((info && 0xff) == 1)
			doagain = 1;
		else
			doagain = 0;
		srp = rp = fdfindsync(crp, erp);
	}
	return(0);
}

/*
 * encode len longwords of `dp' data in amiga mfm block format (`rp')
 * this format specified that the odd bits are at current pos and even
 * bits at len + current pos
 */
u_long *
mfmblkencode(dp, rp, cp, len)
	u_long *dp, *rp, *cp;
	int len;
{
	u_long *sdp, *edp, d, dtmp, correct;
	int i;
	
	sdp = dp;
	edp = dp + len;

	if (*(rp - 1) & 0x1)
		correct = 1;
	else
		correct = 0;
	/*
	 * do odd bits
	 */
	while (dp < edp) {
		d = (*dp >> 1) & 0x55555555;	/* remove clock bits */
		dtmp = d ^ 0x55555555;
		d |= ((dtmp >> 1) | 0x80000000) & (dtmp << 1);
		/*
		 * correct upper clock bit if needed
		 */
		if (correct)
			d &= 0x7fffffff;
		if (d & 0x1)
			correct = 1;
		else
			correct = 0;
		/*
		 * do checksums and store in raw buffer
		 */
		if (cp)
			*cp ^= d;
		*rp++ = d;
		dp++;
	}
	/*
	 * do even bits
	 */
	dp = sdp;
	while (dp < edp) {
		d = *dp & 0x55555555;	/* remove clock bits */
		dtmp = d ^ 0x55555555;
		d |= ((dtmp >> 1) | 0x80000000) & (dtmp << 1);
		/*
		 * correct upper clock bit if needed
		 */
		if (correct)
			d &= 0x7fffffff;
		if (d & 0x1)
			correct = 1;
		else
			correct = 0;
		/*
		 * do checksums and store in raw buffer
		 */
		if (cp)
			*cp ^= d;
		*rp++ = d;
		dp++;
	}
	if (cp)
		*cp &= 0x55555555;
	return(rp);
}

/*
 * decode len longwords of `dp' data in amiga mfm block format (`rp')
 * this format specified that the odd bits are at current pos and even
 * bits at len + current pos
 */
u_long *
mfmblkdecode(rp, dp, cp, len)
	u_long *rp, *dp, *cp;
	int len;
{
	u_long o, e;
	int cnt;

	cnt = len;
	while (cnt--) {
		o = *rp;
		e = *(rp + len);
		if (cp) {
			*cp ^= o;
			*cp ^= e;
		}
		o &= 0x55555555;
		e &= 0x55555555;
		*dp++ = (o << 1) | e;
		rp++;
	}
	if (cp)
		*cp &= 0x55555555;
	return(rp + len);
}