static char rcsid[] = "$Header: fd.c,v 800.0 85/08/06 14:12:40 root Exp $";
static char sccsid[]="%W% %Y% %Q% %G%";

/************************************************************************
*									*
*				Copyright 1984				*
*			VALID LOGIC SYSTEMS INCORPORATED		*
*									*
*	This listing contains confidential proprietary information	*
*	which is not to be disclosed to unauthorized persons without	*
*	written consent of an officer of Valid Logic Systems 		*
*	Incorporated.							*
*									*
*	The copyright notice appearing above is included to provide	*
*	statutory protection in the event of unauthorized or 		*
*	unintentional public disclosure.				*
*									*
************************************************************************/

/*
 * hacked on by sbs 7-21, 9-13
 * sas 841019-22
 */


/* #define BUG */

#include "../h/param.h"
#include "../white/config.h"
#include "../h/dir.h"
#include "../h/buf.h"
#include "../h/conf.h"
#include "../h/user.h"
#include "../h/systm.h"
#include "../white/iobuf.h"
#include "../h/ioctl.h"
#include "../white/diskformat.h"

/* XXX */

#include "../white/fd.h"
#ifdef stupid
#define	btod(x)	(((x)+511)/512)		/* BUG kludge */
#else stupid
#define	btod(x)	(((x)+511)>>9)		/* BUG kludge */
#endif stupid

/*
 * floppy boot partition
 * sector # 208 starts partition b
 */
u_long	fd_offset = 208;		/* from conf.ven.c */

int	fd_vpage;
extern	pageshift, pagemask;
short	fd_debug;			/* debugging flag */

/*
 * physio passes physical addresses to fdstrategy; allocate some virtual
 * space to map into
 */
fdinit() {
	fd_vpage = kvalloc(2);
}

fdopen(dev,flag)
	dev_t dev;
{
	register struct fdregs *reg = fd_addr(dev);
	register struct fd_type *f = &fd_type;
	register drive = fd_drive (dev);
	register slot = fd_slot(dev);

	reg->ctlLOCAL = 0;
	fdtab.b_flags |= B_TIME;

	if (slot > NSLOT || drive > NDRIVE) {
		printf("open: bad device - dev=%d\n",dev);
		return ENXIO;
	}

	if (f->f_size == 0)  {
		if (reg->ctlLOCAL & N_MINI)
			fdtype(dev,2,2,256,77);
		else
			fdtype(dev,2,2,256,80);
	}

	if (fdreset(dev)) {
		printf("open: cannot reset device \n");
		return EIO;
	}

	if (specify(dev,SRT,HUT,HLT,ND)) {
		return ENXIO;
	}

	if (setlocal(dev,MOTOR_ON,DMA_RD)) {
		printf("open: timed out in setlocal\n");
		return ENXIO;
	}

	fdtab.b_flags |= B_ONCE;
	fdtab.b_dev = dev;

	return;
}


fdreset(dev)
	dev_t dev;
{
	struct fdregs *reg = fd_addr(dev);
	register int j;

	fdtab.io_s2 = 0;

	reg->ctlLOCAL = RESET ;

	j=50;
	do {
		;
	} while (--j != 0);

	reg->ctlLOCAL = 0;

	if ( (reg->ctlLOCAL & N_IRQ) == 0 ) {
		printf("reset: calling senseint\n");
		if (senseint(dev)) {
			printf("fdreset: timeout in senseint\n");
			return(1);
		}
	}

	return(0);
}


fdtype(dev, nside, dens, bytsec, cyldsk)
	dev_t dev;
	short nside, dens, bytsec, cyldsk;
{
	struct fdregs *reg = fd_addr(dev);
	struct fd_type *f = &fd_type;
	int sectors;

	f->f_size = (reg->ctlLOCAL & N_MINI) ? 8 : 5;

	if (nside > 0 && nside <= 2) {
	    f->f_trkcyl = nside;
	} else {
	    printf("%d not a valid # of sides\n",nside);
	    return(1);
	}

	if ((dens < 3) && (dens > 0)) {
	    f->f_dens = dens;
	} else {
	    printf("%d not a vaild density\n",dens);
	    return(1);
	}

	if (f->f_size == 8) {

	    f->f_cyldsk = 77;

	    f->f_sectrk = 26;

	    if (dens == 2)
		f->f_bytsec = 256;
	    else
		f->f_bytsec = 128;

	    f->f_gapln = GAPS[bytsec>>8][f->f_dens-1];

	} else {

	    if ((cyldsk == 40) || (cyldsk == 80)) {
	        f->f_cyldsk = cyldsk;
	    } else {
		printf("%d not valid # of cylinders for 5 1/4in. disk\n",
						cyldsk);
		return(1);
	    }

	    switch (bytsec) {
		case 128:
		    if (dens == 1)
		    	f->f_sectrk = 16;
		    else {
		    	printf("%d not valid sector size for ",dens);
			printf("double density 5 1/4in. disk\n");
			return(1);
		    }
		    break;

		case 256:
		    if (dens == 1)
		    	f->f_sectrk = 8;
		    else
		    	f->f_sectrk = 16;
		    break;

		case 512:
		    if (dens == 1)
		    	f->f_sectrk = 4;
		    else
		    	f->f_sectrk = 8;
		    break;

		default:
		    printf("%d not a valid sector size\n",bytsec);
		    return(1);
	    }

	    f->f_bytsec = bytsec;

	    f->f_gapln = GAPS[(bytsec>>8)+3][f->f_dens-1];

	}

#ifdef BUG
	printf("size=%d, nside=%d, dens=%d, gapln=%x\n",
	   f->f_size,f->f_trkcyl,f->f_dens,f->f_gapln);
	printf("bytsec=%d, sectrk=%d, trkcyl=%d, cyldsk=%d\n",
	   f->f_bytsec,f->f_sectrk,f->f_trkcyl,f->f_cyldsk);
#endif

	return(0);
}


fdclose(dev,flag)
	dev_t dev;
{
	struct fdregs *reg = fd_addr(dev);
	unsigned char b;

	while ((fdtab.b_active) || (fdtab.io_s1==C_RECAL)) {
		sleep(&fdtab,PRIBIO);
	}

	fdtab.b_flags = 0;
	fdtab.io_s2 = 0;
	fdtab.io_s1 = 0;

	fdreset(dev);

	/*
	b = 0;
	b |= fd_drive(dev);
	b &= (MOTOR_OFF & ~DRV_SEL & ~ENBL_INT);
	reg->ctlLOCAL = b;
	*/
	setlocal(dev,MOTOR_OFF,DMA_RD);


	return;
}


fdstrategy(bp)
	register struct buf *bp;
{
	long diskbyts;
	register struct fd_type *f = &fd_type;

	fdtab.b_dev = bp->b_dev;

	if (fdtab.b_flags & IO_ERROR) {
#ifdef BUG
		printf("strategy: flushing buffer - bp=%x\n",bp);
#endif
		goto sbad;
	}

	if  (bp == &fdcbuf) {
		bp->b_resid = 0;
		goto iocmd;
	}

	diskbyts = f->f_bytsec * f->f_sectrk * f->f_trkcyl * 
							f->f_cyldsk;
	
	if (bp->b_blkno < 0 || 
	    fd_off(bp->b_dev)+bp->b_blkno > btod(diskbyts)) {
		printf("fdstrategy: illegal blkno - blk=%d  dskblks=%d\n",
			bp->b_blkno, btod(diskbyts));
		goto sbad;
	}

	if ( dtob(fd_off(bp->b_dev)+bp->b_blkno) + 
				bp->b_bcount > diskbyts )  {
#ifdef BUG
		printf("strat: transfer being trimmed - ");
		printf("blkno=%d,old bcount=%d,",bp->b_blkno,bp->b_bcount);
#endif
		bp->b_bcount = diskbyts-dtob(fd_off(bp->b_dev)+bp->b_blkno);
#ifdef BUG
		printf("new bcount=%d\n",bp->b_bcount);
#endif
	}

	/* argument to disksort */
	bp->b_resid = fd_off(bp->b_dev)+bp->b_blkno;

#ifdef BUG
	printf("strat: bcount=%d,blkno=%d,blkxfer=%d,diskbyts=%d\n",
		bp->b_bcount,bp->b_blkno,blkxfer,diskbyts);
#endif

iocmd:	spl2();	/* was 1, but interrupt levels changed */
	disksort(&fdtab, bp);

	if ((fdtab.b_active==0) && (fdtab.io_s1!=C_RECAL)){
		fdstart();
	}

	spl0();
	return;

sbad:	bp->b_flags |= B_ERROR;
	iodone(bp);
	return;
}


fdstart()
{
	register struct buf *bp;
	register struct fd_type *f = &fd_type;
	int state,dtl;
	int sn,tn,cn;

loop:
	if ((bp = fdtab.b_actf) == (struct buf *)NULL) {
		fdtab.b_active = 0;
		if (fdtab.b_flags & B_TIME) {	
			wakeup(&fdtab);
		}
		return;
	}

	if (fdtab.b_flags & IO_ERROR) {
#ifdef BUG
		printf("start: flushing buffer - bp=%x\n",bp);
#endif
		goto bad;
	}

	if ( fdtab.b_flags & B_ONCE ) {
	    fdtab.b_flags &= ~B_ONCE;
	    if ((bp==&fdcbuf) && (bp->b_bcount==C_READ_ID))
		    goto label;
	    if (recal(bp->b_dev)) goto bad;
	    return;
	}

label:	if (fdtab.b_active == 0) {
	    fdtab.b_active = S_IDLE;

	    if (bp == &fdcbuf)
		switch (bp->b_bcount) {
		    case C_FORMAT:
		    case C_VERIFY:
			bp->b_resid = (f->f_trkcyl*f->f_ctl);
			break;

		    case C_SEEK:
		    case C_READ_ID:
			bp->b_resid = 1;
			break;

		    default:
			printf("start: invalid ioctl command %x\n",
					bp->b_bcount);
			goto bad;
		}
	    else
		bp->b_resid = bp->b_bcount;
	}

	state = fdtab.b_active;

	if ((int)bp->b_resid <= 0) 
		goto next;

	if (bp == &fdcbuf) {
		switch (bp->b_bcount) {
		case C_READ_ID:
			sn = 0;
			tn = (f->f_ctl>>8) & 0xFF;
			cn = 0;
			state = S_SEEK;
			break;
		case C_SEEK:
			sn = 0;
			tn = (f->f_ctl>>8) & 0xFF;
			cn = f->f_ctl & 0xFF;
			break;
		case C_FORMAT:
		case C_VERIFY:
			sn = 0;
			tn = ((f->f_trkcyl*f->f_ctl) - bp->b_resid)
							% f->f_trkcyl;
			cn = ((f->f_trkcyl*f->f_ctl) - bp->b_resid) 
							/ f->f_trkcyl;
			break;
		default:
			goto bad;
		}
		goto swtch;
	}

	dtl = ( (bp->b_resid < f->f_bytsec ) ? bp->b_resid : 0xff);

	fdoffset = bp->b_bcount - bp->b_resid;

	sn = ( dtob(fd_off(bp->b_dev)+bp->b_blkno) + fdoffset ) / f->f_bytsec;

	tn = (sn / f->f_sectrk) % f->f_trkcyl;
	cn = (sn / f->f_sectrk) / f->f_trkcyl;
	sn = sn % f->f_sectrk + 1;
	
swtch:	
#ifdef BUG
	printf("blk=%d, sn=%d, tn=%d, cn=%d, resid=%d, state=%d\n ",
		bp->b_blkno+fd_off(bp->dev),sn,tn,cn,bp->b_resid,state);
#endif
			
	switch (state) {
	case S_IDLE:
	case S_XFER:
		fdtab.b_active = S_SEEK;
		if (oldcyl != cn) {
			if (fdseek(bp->b_dev,tn,cn)) goto bad;
			break;
		}

	case S_SEEK:
		fdtab.b_active = S_XFER;
		if (bp == &fdcbuf) {
			switch (bp->b_bcount) {
			case C_FORMAT:
				if (fdformat(bp,cn,tn)) goto bad;
				break;
			case C_VERIFY:
				if (verify(bp,cn,tn)) goto bad;
				break;
			case C_SEEK:
				goto next;
			case C_READ_ID:
				if (read_id(bp->b_dev,tn)) goto bad;
				break;
			default:
				goto bad;
			}
		} else {
			if (fdrw(bp,dtl,sn,tn,cn,
				bp->b_flags & B_PHYS ? (int)bp->b_un.b_addr :
						       vtop(bp->b_un.b_addr))) 
					 goto bad;
		}
		break;
	default:
		printf("fdstart: bad state=%d\n",fdtab.b_active);
		goto bad;
	}
	return;

bad:
	bp->b_flags |= B_ERROR;
	fdtab.b_flags |= IO_ERROR;
#ifdef BUG
	printf("fdstart: error in transfer - bp=%x\n",bp);
#endif

next:
	fdtab.b_active = 0;
	fdtab.io_s1 = 0;
	fdtab.b_errcnt = 0;
	fdtab.b_actf = bp->av_forw;

	iodone(bp);
	goto loop;
}


recal(dev)
	dev_t dev;
{
	static unsigned char cmd[2];
	register struct fdregs *reg = fd_addr(dev);
	
	cmd[0] = C_RECAL;
	cmd[1] = fd_drive(dev);

	if ( setlocal(dev,MOTOR_ON,DMA_RD) ) {
		printf("recal: timeout in setlocal\n");
		return(1);
	}

	if ( (reg->ctlLOCAL & N_IRQ) == 0) {
		if (senseint(dev)) {
			printf("recal: timeout in senseint\n");
			return(1);
		}
	}

	oldcyl = 0;

	fdtab.io_s2 |= ENBL_INT;
	reg->ctlLOCAL = fdtab.io_s2;

#ifdef BUG
	printf("recal: cmd[0]=%x,cmd[1]=%x\n",cmd[0],cmd[1]);
#endif
	if ( wrcmd(dev,cmd,2) ) {
		printf("recal: timeout in wrcmd\n");
		return(1);
	}

	return(0);
}


fdseek(dev,head,cyl)
	dev_t dev;
	int cyl, head;
{
	static unsigned char cmd[3];
	register struct fdregs *reg = fd_addr(dev);
	int i;

	cmd[0] = C_SEEK;
	cmd[1] = fd_drive(dev) | (head << 2);
	cmd[2] = cyl;

#ifdef BUG
	printf("fdseek: command bytes:");
	for (i=0;i<3;i++)
		printf("%d=%x, ",i,cmd[i]);
	printf("\n");
#endif

	if (setlocal(dev,MOTOR_ON,DMA_RD)) {
		printf("seek: timed out in setlocal\n");
		return(1);
	}

	if ( (reg->ctlLOCAL & N_IRQ) == 0) {
		if (senseint(dev)) {
			printf("fdseeek: timeout in senseint\n");
			return(1);
		}
	}

	oldcyl = cyl;

	fdtab.io_s2 |= ENBL_INT;
	reg->ctlLOCAL = fdtab.io_s2;

	if (wrcmd(dev,cmd,3)) {
		printf("seek: timed out on wrcmd\n");
		return(1);
	}


	return(0);
}


fdrw(bp, dtl, sn, tn, cn, addr)
	struct buf *bp;
	int sn, tn, cn;
	caddr_t addr;
{
	register char *wrbuf;
	register struct fdregs *reg = fd_addr(bp->b_dev);
	register struct fd_type *f = &fd_type;
	static unsigned char cmd[9];
	int secln, bufcnt, i;

	cmd[0] = (bp->b_flags & B_READ) ? C_READ : C_WRITE;
	if (f->f_dens == 2) 
		cmd[0] |= C_MFM_BIT;

	cmd[1] = (tn<<2) | fd_drive(bp->b_dev);
	cmd[2] = cn;
	cmd[3] = tn;	
	cmd[4] = sn;

	secln = f->f_bytsec >> 8;
	cmd[5] = secln;
	cmd[6] = f->f_sectrk;
	cmd[7] = (f->f_gapln >> 8) & 0xFF;
	cmd[8] = dtl;

#ifdef BUG
	printf("fdrw: ");
	for (i=0;i<9;i++)
		printf("%d=%x, ",i,cmd[i]);
	printf("\n");
#endif

	if(setlocal(bp->b_dev,MOTOR_ON,((bp->b_flags&B_READ)?DMA_RD:DMA_WR))){
		printf("fdrw: timed out in setlocal\n");
		return(1);
	}

	bufcnt = (bp->b_resid <= f->f_bytsec) ? 
			bp->b_resid : f->f_bytsec;
	reg->ctlCNTR = (bufcnt / 2) - 1;

	if ( (bp->b_flags & B_READ) == 0 ) {
		wrbuf = (char *)addr + fdoffset;
#ifdef WHITE
		setpagemap(fd_vpage, (int)wrbuf >> pageshift);
		setpagemap(fd_vpage + 1, ((int)wrbuf >> pageshift) + 1);
		wrbuf = (char *)((fd_vpage<<pageshift) + ((int)wrbuf&pagemask));
#endif WHITE
		i=bufcnt;
		do
			reg->ctlBUF = *wrbuf++;
		while ( --i > 0);
	
		reg->ctlCNTR = (bufcnt / 2) - 1;

	}

	if ( (reg->ctlLOCAL&N_IRQ) == 0) {
		if (senseint(bp->b_dev)) {
			printf("fdrw: timeout in senseint\n");
			return(1);
		}
	}

	fdtab.io_s2 |= ENBL_INT;
	reg->ctlLOCAL = fdtab.io_s2;
	
	if (wrcmd(bp->b_dev, cmd, 9) ) {
		printf("read: timed out in wrcmd\n");
		return(1);
	}

	return(0);
}


fdint(unit)
{
	register struct buf *bp;
	register struct fdregs *reg = fd_addr(fdtab.b_dev);
	register struct fd_type *f = &fd_type;
	register char *rdbuf;
	caddr_t addr;
	static unsigned char cmd[7];
	unsigned char  C, H, R, N;
	int bufcnt, i;
	unsigned int secid;

	fdtab.io_s2 &= ~ENBL_INT;
	reg->ctlLOCAL = fdtab.io_s2;

	if ((bp = fdtab.b_actf) == (struct buf *)NULL ) {
		printf("fdint: b_actf == 0  slot=%d\n", unit);
		return;
	}
	addr = (caddr_t) (bp->b_flags & B_PHYS ?
				(int)bp->b_un.b_addr :
				vtop(bp->b_un.b_addr));

#ifdef BUG
	printf("fdint: starting switch - cmd=%x\n",fdtab.io_s1);
#endif
	switch (fdtab.io_s1 & 0x0F) {
	case C_READ:
	case C_WRITE:
		if (reg->ctlSTATUS&FDC_BUSY == 0) {
#ifdef BUG
			printf("fdint: wrong interrupt for read/write\n");
#endif
			if (senseint(fdtab.b_dev)) {
				printf("fdint: timeout in senseint\n");
				goto err;
			}
			fdtab.io_s2 |= ENBL_INT;
			reg->ctlLOCAL = fdtab.io_s2;
			return;
		}
		if (!rdcmd(bp->b_dev,cmd,7) ) {
			ST0 = cmd[0];
			ST1 = cmd[1];
			ST2 = cmd[2];
			C = cmd[3];
			H = cmd[4];
			R = cmd[5];
			N = cmd[6];
			if ( ((INT_CODE0|INT_CODE1) & ST0) == 0) {
				if (bp->b_flags & B_READ) {
					rdbuf = (char *)addr + fdoffset;
#ifdef WHITE
					setpagemap(fd_vpage,
						(int)rdbuf >> pageshift);
					setpagemap(fd_vpage + 1,
						((int)rdbuf >> pageshift) + 1);
					rdbuf = (char *)((fd_vpage << pageshift)
						+ ((int)rdbuf & pagemask));
#endif WHITE
					bufcnt = (bp->b_resid <= f->f_bytsec)?
							bp->b_resid:f->f_bytsec;
					reg->ctlCNTR = (bufcnt / 2) - 1;
					i=bufcnt;
					do {
						*rdbuf++ = reg->ctlBUF;
					} while (--i > 0);
				}

				bp->b_resid = (bp->b_resid <= f->f_bytsec) ?
					0 : (bp->b_resid - f->f_bytsec);
				goto last;
			}

			if (ST1 & ( LOST_ADD_MRK | DATA_ERR)) {
#ifdef BUG
				printf("fdint: retry - ST1=%x, resid=%d\n",
				    ST1,bp->b_resid);
#endif BUG
				fdtab.b_active = S_SEEK;
				if (++fdtab.b_errcnt <= 7) {
					goto last;
				} else {
					fdtab.b_flags |= IO_ERROR;
					fdtab.b_errcnt = 0;
				}
			}
#ifdef BUG
			printf("fdint: cmd=%x, ST0=%x, ST1=%x, ST2=%x, ",
					fdtab.io_s1,ST0,ST1,ST2);
			printf("C=%d, H=%d, R=%d, N=%d\n",C,H,R,N);
#endif BUG
			/*
			 * can do better with %b perhaps?
			 */
			if (FD_N_RDY & ST0)
				printf("floppy drive not ready\n");
			if (EQUIP_CK & ST0)
				printf("floppy drive equip fault\n");
			if (LOST_ADD_MRK & ST1)
				printf("missing data addr mark on floppy\n");
			if (SEC_N_FND & ST1)
				printf("sector not found on floppy\n");
			if (WR_PROT & ST1)
				printf("floppy is write protected\n");
			if (DATA_ERR & ST1)
				printf("CRC error detected on floppy\n");
			if (BAD_TRK & ST2)
				printf("bad track error on floppy\n");
			if (WRONG_CYL & ST2)
				printf("cylinder addr error on floppy\n");
			if (CTL_MARK & ST2)
				printf("unexpected control mark on floppy\n");
		}
		goto err;

	case C_FORMAT:
	case C_VERIFY:
		if ( !rdcmd(bp->b_dev,cmd,7) ) {
			ST0 = cmd[0];
			ST1 = cmd[1];
			ST2 = cmd[2];
			if ( ((INT_CODE0|INT_CODE1) & ST0) == 0) {
				bp->b_resid -= 1;
				goto last;
			}
			printf("fdint: cmd=%x, ST0=%x, ST1=%x, ST2=%x\n",
					fdtab.io_s1,ST0,ST1,ST2);
		}
		goto err;

	case C_RECAL:
	case C_SEEK:
		if (senseint(fdtab.b_dev)) {
			printf("fdint: timeout in senseint\n");
			goto err;
		}
		if ( (ST0 & (SEEK_END|INT_CODE0)) == SEEK_END) {
			if (ST1 == oldcyl) {
				fdtab.b_errcnt = 0;
				goto last;
			}
		}
#ifdef BUG
		printf("fdint: seek error - ST0=%x, cyl#=%x\n",ST0,ST1);
#endif
		oldcyl = -1;
		if (++fdtab.b_errcnt > 3) {
		    	printf("fdint: 3 fails on seek-ST0=%x\n",ST0);
			fdtab.b_errcnt = 0;
			goto err;
		}
		if (fdtab.b_active != 0)
		        fdtab.b_active = S_IDLE;
		fdtab.b_flags |= B_ONCE;
		goto last;

	case C_READ_ID:
		if (rdcmd(bp->b_dev,cmd,7)) {
			printf("fdint: timedout in rdcmd for read_id\n");
			goto err;
		}
		ST0 = cmd[0];
		ST1 = cmd[1];
		ST2 = cmd[2];
		C = cmd[3];
		H = cmd[4];
		R = cmd[5];
		N = cmd[6];
		bp->b_resid -= 1;
		printf("read_id: ST0=%x, ST1=%x, ST2=%x, ",ST0,ST1,ST2);
		printf("Cyl=%d,Head=%d,Sec=%d,sec sz=%d\n",C,H,R,N);
		goto last;

	default:
		printf("fdint: unknown command - %x hex\n",fdtab.io_s1);
		goto err;
	}
	/*NOTREACHED*/

err:
	fdtab.b_active = 0;
	fdtab.b_actf = bp->av_forw;
	fdtab.b_flags |= IO_ERROR;
	bp->b_flags |= B_ERROR;
	printf("fdint calling iodone-bp=%x\n",bp);
	iodone(bp);
last:
	fdstart();
}



fdread(dev, uio)
	dev_t	dev;
	struct	uio	*uio;
{
	return physio(fdstrategy, &fdrbuf, dev, B_READ, minphys, uio);
}


fdwrite(dev, uio)
	dev_t	dev;
	struct	uio	*uio;
{
	return physio(fdstrategy, &fdrbuf, dev, B_WRITE, minphys, uio);
}


fdioctl(dev, cmd, data, flag)
	dev_t	dev;
	int 	cmd;
	u_char	*data;
	int 	flag;
{
	register struct buf *bp;
	register struct fd_type *f = &fd_type;
	register struct fd_type *addr = (struct fd_type *) data;
	register int fdcmd;
	int s;

	/*
	 * This is really stupid...
	 */
	switch (cmd) {
	case FDIOCRESET:
		fdcmd = C_RESET;
		break;
	case FDIOCSETTYPE:
		fdcmd = C_SET_TYPE;
		break;
	case FDIOCGETTYPE:
		fdcmd = C_TELL_TYPE;
		break;
	case FDIOCSEEK:
		fdcmd = C_SEEK;
		break;
	case FDIOCFORMAT:
		fdcmd = C_FORMAT;
		break;
	case FDIOCVERIFY:
		fdcmd = C_VERIFY;
		break;
	case FDIOCREADID:
		fdcmd = C_READ_ID;
		break;
	default:
		return(ENOTTY);
	}


	if (fd_debug)
		printf("fdioctl: dev=%d, command=%x\n",dev,cmd);

	switch (cmd) {
	case FDIOCRESET:
		fdreset(dev);
		return;
	case FDIOCSETTYPE:
		if (fdtype(dev, addr->f_trkcyl, addr->f_dens, addr->f_bytsec,
		    addr->f_cyldsk))
			goto errxit;
		bcopy(&fd_type, data, sizeof(struct fd_type));
		break;
	case FDIOCGETTYPE:
		bcopy(&fd_type, data, sizeof(struct fd_type));
		break;
	case FDIOCSEEK:
	case FDIOCFORMAT:
	case FDIOCVERIFY:
	case FDIOCREADID:
		bp = &fdcbuf;
		/*
		 * Added this to ensure that b_flags is correct
		 * sas 841010
		 */
		bp->b_flags = B_BUSY;

		bp->b_dev = dev;
		fdtab.io_s1 = fdcmd;
		bp->b_bcount = fdcmd;

		f->f_ctl = addr->f_ctl;
		if ((f->f_ctl & 0xFF) > f->f_cyldsk){
			printf("%d out of range for cylinder #",
					(f->f_ctl & 0xFF));
			goto errxit;
		}
		if ((f->f_ctl >> 8) > 1) {
			printf("%d out of range for track #",
					(f->f_ctl >> 8));
			goto errxit;
		}
		fdstrategy(bp);
		iowait(bp);

		bp->b_flags = 0;

		return(u.u_error);	/* yuck */

		break;
	default:
		return(ENOTTY);		/* yuck */
	}
	return(0);
errxit:	
	return(EIO);
}




fdformat(bp,cn,tn)
	register struct buf *bp;
	int cn,tn;
{
	register struct fd_type *f = &fd_type;
	register struct fdregs *reg = fd_addr(bp->b_dev);
	static unsigned char cmd[6];
	unsigned char b;
	int i,j, s, secln, count;

	/* set up cmd for format */
	cmd[0] = C_FORMAT;

	if (f->f_dens == 2)
		cmd[0] |= C_MFM_BIT;

	secln = f->f_bytsec >> 8;

	cmd[1] = (tn<<2) | fd_drive(bp->b_dev);
	cmd[2] = secln;
	cmd[3] = f->f_sectrk;
	cmd[4] = f->f_gapln & 0xFF;
	cmd[5] = 0x1A;

	count = ((f->f_sectrk<<2)-1)/2;

#ifdef BUG
	printf("frmt: "); 
	for (i=0;i<6;i++)
		printf("%d=%x, ",i,cmd[i]);
	printf("count=%d\n",count);
#endif

	reg->ctlCNTR = count;
	for (i=1; i<=f->f_sectrk; i++) {
		reg->ctlBUF = cn;
		reg->ctlBUF = tn;
		reg->ctlBUF = i;
		reg->ctlBUF = secln;
	}

	reg->ctlCNTR = count;
	setlocal(bp->b_dev,MOTOR_ON,DMA_WR);
	
	if ( (reg->ctlLOCAL&N_IRQ) == 0) {
		if (senseint(bp->b_dev)) {
			printf("format: timeout in senseint\n");
			return(1);
		}
	}

	fdtab.io_s2 |= ENBL_INT;
	reg->ctlLOCAL = fdtab.io_s2;

	if (wrcmd(bp->b_dev,cmd,6)) {
		printf("format: timed out on wrcmd\n");
		return(1);
	}
	return(0);
}


verify(bp,cn,tn)
	register struct buf *bp;
	int cn, tn;
{
	register struct fdregs *reg = fd_addr(bp->b_dev);
	register struct fd_type *f = &fd_type;
	static unsigned char cmd[9];
	int secln, bufcnt, i;

	cmd[0] = C_VERIFY;
	if (f->f_dens == 2) 
		cmd[0] |= C_MFM_BIT;

	cmd[1] = (tn<<2) | fd_drive(bp->b_dev);
	cmd[2] = cn;
	cmd[3] = tn;	
	cmd[4] = 1;

	secln = f->f_bytsec >> 8;	 /* n=sec size code */
	cmd[5] = secln;
	cmd[6] = f->f_sectrk;
	cmd[7] = (f->f_gapln >> 8) & 0xFF;
	cmd[8] = 0xFF;

#ifdef BUG
	printf("verify: command bytes are:\n");
	for (i=0;i<9;i++)
		printf("%d=%x, ",i,cmd[i]);
	printf("\n");
#endif

	setlocal(bp->b_dev,MOTOR_ON,DMA_RD);

	if ( (reg->ctlLOCAL&N_IRQ) == 0) {
		if (senseint(bp->b_dev)) {
			printf("verify: timeout in senseint\n");
			return(1);
		}
	}

	fdtab.io_s2 |= ENBL_INT;
	reg->ctlLOCAL = fdtab.io_s2;
	
	if ( wrcmd(bp->b_dev,cmd,9) ) {
		printf("verify: timed out in wrcmd\n");
		return(1);
	}
	return(0);
}


read_id(dev, tn)
	dev_t	dev;
{
	struct fd_type *f = &fd_type;
	struct fdregs *reg = fd_addr(dev);
	static unsigned char cmd[2];

	cmd[0] = C_READ_ID;
	if (f->f_dens == 2)
		cmd[0] |= C_MFM_BIT;
	cmd[1] = (tn<<2) | fd_drive(dev);

	setlocal(dev,MOTOR_ON,DMA_RD);
	fdtab.io_s2 |= ENBL_INT;
	reg->ctlLOCAL = fdtab.io_s2;

	if (wrcmd(dev, cmd, 2)) {
		printf("read_id: timedout in wrcmd\n");
		return(1);
	}
	return(0);
}



specify(dev, srt, hut, hlt, nd)
	dev_t dev;
{
	static unsigned char cmd[3];

	cmd[0] = C_SPEC;
	cmd[1] = (srt << 4) | hut;
	cmd[2] = (hlt << 1) | nd;
	if (wrcmd(dev, cmd, 3) ) {
		printf("specify: timed out on wrcmd\n");
		return(1);
	}
	return(0);
}



setlocal(dev, motor, DMAdir)
	dev_t dev;
{
	struct fdregs *reg = fd_addr(dev);
	int select, reset, i, j;

	select = motor ? 0 : DRV_SEL ;
	fdtab.io_s2 = ( fd_drive(dev) | select) | motor | DMAdir;

	reg->ctlLOCAL = fdtab.io_s2;

#ifdef BUG
	printf("setlocal: control byte = %x\n", fdtab.io_s2);
#endif

	if (motor) 
		return(0);
	for (i=0; i<TIMEOUT; i++) {
		if ( (N_DRV_RDY & reg->ctlLOCAL) == 0)  {
			return(0);
		}
	} 
	printf("drive is not ready\n");
	return(1);
}



senseint(dev)
	dev_t	dev;
{
	static unsigned char cmd[2];

	cmd[0] = C_SIS;
	if (wrcmd(dev,cmd,1)) 
		return(1);
	if (rdcmd(dev,cmd,2)) 
		return(1);

	ST0 = cmd[0];
	ST1 = cmd[1];
	return(0);
}



wrcmd(dev,cmd,n)
	dev_t	dev;
	u_char	cmd[];
{
	struct fdregs *reg = fd_addr(dev);
	int i,j;

	for (i=0; i<n; i++) {
		for (j=0; j<TIMEOUT; j++) {
			if ((reg->ctlSTATUS & DREG_RDY) == DREG_RDY ) {
				reg->ctlDATA = cmd[i];
				goto next;
			}
		} 
		printf("wrcmd: timeout-status=%x\n",reg->ctlSTATUS);
		return(1);
next:		continue;
	}
	fdtab.io_s1 = cmd[0];
	return(0);
}


rdcmd(dev, cmd, n)
	dev_t	dev;
	u_char	cmd[];
{
	struct fdregs *reg = fd_addr(dev);
	int i,j;

	for (i=0; i<n; i++) {
		for (j=0; j<TIMEOUT; j++) {
			if ( (reg->ctlSTATUS & (DATA_DIR_RD|DREG_RDY) ) 
					== (DATA_DIR_RD|DREG_RDY)) {
				cmd[i] = reg->ctlDATA;
				goto next;
			}
		} 
		printf("rdcmd: timeout-status=%x\n",reg->ctlSTATUS);
		return(1);
next:		continue;
	}
	return(0);
}
