/*
 * Copyright (c) 1992-1999 by Sun Microsystems, Inc.
 * All rights reserved.
 */

#pragma ident	"@(#)strategy.c	1.27	99/05/20 SMI"

/*
 *	Device Strategy
 */
#include <sys/dktp/cm.h>
#include <sys/kstat.h>
#include <sys/dktp/objmgr.h>

#include <sys/dktp/queue.h>
#include <sys/dktp/tgcom.h>
#include <sys/dktp/flowctrl.h>
#include <sys/dktp/fctypes.h>
#include <sys/dktp/quetypes.h>
#include <sys/param.h>
#include <vm/page.h>
#include <sys/modctl.h>

/*
 *	Object Management
 */
static opaque_t qfifo_create();
static opaque_t qmerge_create();
static opaque_t qsort_create();
static opaque_t qtag_create();
static opaque_t dsngl_create();
static opaque_t dmult_create();
static opaque_t duplx_create();
static opaque_t adapt_create();
static struct buf *qmerge_nextbp(struct que_data *qfp, struct buf *bp_merge,
    int *can_merge);

static struct modlmisc modlmisc = {
	&mod_miscops,	/* Type of module */
	"Device Strategy Objects"
};

static struct modlinkage modlinkage = {
	MODREV_1,
	&modlmisc,
	NULL
};

char _depends_on[] = "drv/objmgr";

int
_init(void)
{
	(void) objmgr_ins_entry("qfifo", (opaque_t)qfifo_create, "strategy");
	(void) objmgr_ins_entry("qmerge", (opaque_t)qmerge_create, "strategy");
	(void) objmgr_ins_entry("qsort", (opaque_t)qsort_create, "strategy");
	(void) objmgr_ins_entry("qtag",  (opaque_t)qtag_create, "strategy");
	(void) objmgr_ins_entry("dsngl", (opaque_t)dsngl_create, "strategy");
	(void) objmgr_ins_entry("dmult", (opaque_t)dmult_create, "strategy");
	(void) objmgr_ins_entry("duplx", (opaque_t)duplx_create, "strategy");
	(void) objmgr_ins_entry("adapt", (opaque_t)adapt_create, "strategy");
	return (mod_install(&modlinkage));
}

int
_fini(void)
{
	if ((objmgr_del_entry("qfifo") == DDI_FAILURE) ||
	    (objmgr_del_entry("qmerge") == DDI_FAILURE) ||
	    (objmgr_del_entry("qsort") == DDI_FAILURE) ||
	    (objmgr_del_entry("qtag")  == DDI_FAILURE) ||
	    (objmgr_del_entry("dsngl") == DDI_FAILURE) ||
	    (objmgr_del_entry("dmult") == DDI_FAILURE) ||
	    (objmgr_del_entry("adapt") == DDI_FAILURE) ||
	    (objmgr_del_entry("duplx") == DDI_FAILURE))
		return (EBUSY);
	return (mod_remove(&modlinkage));
}

int
_info(struct modinfo *modinfop)
{
	return (mod_info(&modlinkage, modinfop));
}


/*
 *	Common Flow Control functions
 */

/*
 * Local static data
 */
#ifdef	FLC_DEBUG
#define	DENT	0x0001
#define	DERR	0x0002
#define	DIO	0x0004
static	int	flc_debug = DENT|DERR|DIO;


#include <sys/thread.h>
static 	int	flc_malloc_intr = 0;

#endif	/* FLC_DEBUG */
static	int	flc_kstat = 1;

static opaque_t fc_create(struct flc_objops *fcopsp);
static int fc_init(struct fc_data *fcdp, opaque_t tgcom_objp,
	opaque_t que_objp, void *lkarg);
static int fc_free(struct flc_obj *flcobjp);
static int fc_start_kstat(struct fc_data *fcdp, char *devtype, int instance);
static int fc_stop_kstat(struct fc_data *fcdp);

static opaque_t
fc_create(struct flc_objops *fcopsp)
{
	register struct	flc_obj *flcobjp;
	register struct	fc_data *fcdp;

	flcobjp = (struct flc_obj *)kmem_zalloc((sizeof (*flcobjp) +
		sizeof (*fcdp)), KM_NOSLEEP);
	if (!flcobjp)
		return (NULL);

	fcdp = (struct fc_data *)(flcobjp+1);
	flcobjp->flc_data = (opaque_t)fcdp;
	flcobjp->flc_ops  = fcopsp;

	return ((opaque_t)flcobjp);
}

static int dmult_maxcnt = DMULT_MAXCNT;
static int
fc_init(register struct fc_data *fcdp, opaque_t tgcom_objp, opaque_t que_objp,
	void *lkarg)
{
	mutex_init(&fcdp->ds_mutex, NULL, MUTEX_DRIVER, lkarg);

	fcdp->ds_queobjp   = que_objp;
	fcdp->ds_tgcomobjp = tgcom_objp;
	fcdp->ds_waitcnt   = dmult_maxcnt;

	QUE_INIT(que_objp, lkarg);
	TGCOM_INIT(tgcom_objp);
	return (DDI_SUCCESS);
}

static int
fc_free(struct flc_obj *flcobjp)
{
	register struct fc_data *fcdp;

	fcdp = (struct fc_data *)flcobjp->flc_data;
	if (fcdp->ds_queobjp)
		QUE_FREE(fcdp->ds_queobjp);
	if (fcdp->ds_tgcomobjp) {
		TGCOM_FREE(fcdp->ds_tgcomobjp);
		mutex_destroy(&fcdp->ds_mutex);
	}
	kmem_free(flcobjp, (sizeof (*flcobjp) + sizeof (*fcdp)));
	return (0);
}

/*ARGSUSED*/
static int
fc_start_kstat(struct fc_data *fcdp, char *devtype, int instance)
{
	if (!flc_kstat)
		return (0);

	if (!fcdp->ds_kstat) {
		if (fcdp->ds_kstat = kstat_create("cmdk", instance, NULL,
				"disk", KSTAT_TYPE_IO, 1,
				KSTAT_FLAG_PERSISTENT)) {
			kstat_install(fcdp->ds_kstat);
		}
	}
	return (0);
}

static int
fc_stop_kstat(struct fc_data *fcdp)
{
	if (fcdp->ds_kstat) {
		kstat_delete(fcdp->ds_kstat);
		fcdp->ds_kstat = NULL;
	}
	return (0);
}


/*
 *	Single Command per Device
 */
/*
 * Local Function Prototypes
 */
static int dsngl_restart();

static int dsngl_enque();
static int dsngl_deque();

struct 	flc_objops dsngl_ops = {
	fc_init,
	fc_free,
	dsngl_enque,
	dsngl_deque,
	fc_start_kstat,
	fc_stop_kstat,
	0, 0
};

static opaque_t
dsngl_create()
{
	return (fc_create((struct flc_objops *)&dsngl_ops));
}

static int
dsngl_enque(register struct fc_data *dsnglp, struct buf *in_bp)
{
	register opaque_t tgcom_objp;
	register opaque_t que_objp;

	que_objp   = dsnglp->ds_queobjp;
	tgcom_objp = dsnglp->ds_tgcomobjp;

	if (!in_bp)
		return (0);
	mutex_enter(&dsnglp->ds_mutex);
	if (dsnglp->ds_bp || dsnglp->ds_outcnt) {
		QUE_ADD(que_objp, in_bp);
		if (dsnglp->ds_kstat) {
			kstat_waitq_enter(KSTAT_IO_PTR(dsnglp->ds_kstat));
		}
		mutex_exit(&dsnglp->ds_mutex);
		return (0);
	}
	if (dsnglp->ds_kstat) {
		kstat_waitq_enter(KSTAT_IO_PTR(dsnglp->ds_kstat));
	}
	if (TGCOM_PKT(tgcom_objp, in_bp, dsngl_restart,
		(caddr_t)dsnglp) != DDI_SUCCESS) {

		dsnglp->ds_bp = in_bp;
		mutex_exit(&dsnglp->ds_mutex);
		return (0);
	}
	dsnglp->ds_outcnt++;
	if (dsnglp->ds_kstat)
		kstat_waitq_to_runq(KSTAT_IO_PTR(dsnglp->ds_kstat));
	mutex_exit(&dsnglp->ds_mutex);
	TGCOM_TRANSPORT(tgcom_objp, in_bp);
	return (0);
}

static int
dsngl_deque(register struct fc_data *dsnglp, struct buf *in_bp)
{
	register opaque_t tgcom_objp;
	opaque_t que_objp;
	struct	 buf *bp;

	que_objp   = dsnglp->ds_queobjp;
	tgcom_objp = dsnglp->ds_tgcomobjp;

	mutex_enter(&dsnglp->ds_mutex);
	if (in_bp) {
		dsnglp->ds_outcnt--;
		if (dsnglp->ds_kstat) {
			if (in_bp->b_flags & B_READ) {
				KSTAT_IO_PTR(dsnglp->ds_kstat)->reads++;
				KSTAT_IO_PTR(dsnglp->ds_kstat)->nread +=
					(in_bp->b_bcount - in_bp->b_resid);
			} else {
				KSTAT_IO_PTR(dsnglp->ds_kstat)->writes++;
				KSTAT_IO_PTR(dsnglp->ds_kstat)->nwritten +=
					(in_bp->b_bcount - in_bp->b_resid);
			}
			kstat_runq_exit(KSTAT_IO_PTR(dsnglp->ds_kstat));
		}
	}
	for (;;) {
		if (!dsnglp->ds_bp)
			dsnglp->ds_bp = QUE_DEL(que_objp);
		if (!dsnglp->ds_bp ||
		    (TGCOM_PKT(tgcom_objp, dsnglp->ds_bp, dsngl_restart,
		    (caddr_t)dsnglp) != DDI_SUCCESS) ||
		    dsnglp->ds_outcnt) {
			mutex_exit(&dsnglp->ds_mutex);
			return (0);
		}
		dsnglp->ds_outcnt++;
		bp = dsnglp->ds_bp;
		dsnglp->ds_bp = QUE_DEL(que_objp);
		if (dsnglp->ds_kstat)
			kstat_waitq_to_runq(KSTAT_IO_PTR(dsnglp->ds_kstat));
		mutex_exit(&dsnglp->ds_mutex);

		TGCOM_TRANSPORT(tgcom_objp, bp);

		if (!mutex_tryenter(&dsnglp->ds_mutex))
			return (0);
	}
}

static int
dsngl_restart(struct fc_data *dsnglp)
{
	(void) dsngl_deque(dsnglp, NULL);
	return (-1);
}


/*
 *	Multiple Commands per Device
 */
/*
 * Local Function Prototypes
 */
static int dmult_restart();

static int dmult_enque();
static int dmult_deque();

struct 	flc_objops dmult_ops = {
	fc_init,
	fc_free,
	dmult_enque,
	dmult_deque,
	fc_start_kstat,
	fc_stop_kstat,
	0, 0
};

static opaque_t
dmult_create()
{
	return (fc_create((struct flc_objops *)&dmult_ops));

}


/*
 * Some of the object management functions QUE_ADD() and QUE_DEL()
 * do not accquire lock.
 * They depend on dmult_enque(), dmult_deque() to do all locking.
 * If this changes we have to grab locks in qmerge_add() and qmerge_del().
 */
static int
dmult_enque(register struct fc_data *dmultp, struct buf *in_bp)
{
	register opaque_t tgcom_objp;
	register opaque_t que_objp;

	que_objp   = dmultp->ds_queobjp;
	tgcom_objp = dmultp->ds_tgcomobjp;

	if (!in_bp)
		return (0);
	mutex_enter(&dmultp->ds_mutex);
	if ((dmultp->ds_outcnt >= dmultp->ds_waitcnt) || dmultp->ds_bp) {
		QUE_ADD(que_objp, in_bp);
		if (dmultp->ds_kstat) {
			kstat_waitq_enter(KSTAT_IO_PTR(dmultp->ds_kstat));
		}
		mutex_exit(&dmultp->ds_mutex);
		return (0);
	}
	if (dmultp->ds_kstat) {
		kstat_waitq_enter(KSTAT_IO_PTR(dmultp->ds_kstat));
	}

	if (TGCOM_PKT(tgcom_objp, in_bp, dmult_restart,
		(caddr_t)dmultp) != DDI_SUCCESS) {

		dmultp->ds_bp = in_bp;
		mutex_exit(&dmultp->ds_mutex);
		return (0);
	}
	dmultp->ds_outcnt++;
	if (dmultp->ds_kstat)
		kstat_waitq_to_runq(KSTAT_IO_PTR(dmultp->ds_kstat));
	mutex_exit(&dmultp->ds_mutex);

	TGCOM_TRANSPORT(tgcom_objp, in_bp);
	return (0);
}

static int
dmult_deque(register struct fc_data *dmultp, struct buf *in_bp)
{
	register opaque_t tgcom_objp;
	opaque_t que_objp;
	struct	 buf *bp;

	que_objp   = dmultp->ds_queobjp;
	tgcom_objp = dmultp->ds_tgcomobjp;

	mutex_enter(&dmultp->ds_mutex);
	if (in_bp) {
		dmultp->ds_outcnt--;
		if (dmultp->ds_kstat) {
			if (in_bp->b_flags & B_READ) {
				KSTAT_IO_PTR(dmultp->ds_kstat)->reads++;
				KSTAT_IO_PTR(dmultp->ds_kstat)->nread +=
					(in_bp->b_bcount - in_bp->b_resid);
			} else {
				KSTAT_IO_PTR(dmultp->ds_kstat)->writes++;
				KSTAT_IO_PTR(dmultp->ds_kstat)->nwritten +=
					(in_bp->b_bcount - in_bp->b_resid);
			}
			kstat_runq_exit(KSTAT_IO_PTR(dmultp->ds_kstat));
		}
	}

	for (;;) {

#ifdef	FLC_DEBUG
		if ((curthread->t_intr) && (!dmultp->ds_bp) &&
		    (!dmultp->ds_outcnt))
			flc_malloc_intr++;
#endif

		if (!dmultp->ds_bp)
			dmultp->ds_bp = QUE_DEL(que_objp);
		if (!dmultp->ds_bp ||
		    (TGCOM_PKT(tgcom_objp, dmultp->ds_bp, dmult_restart,
		    (caddr_t)dmultp) != DDI_SUCCESS) ||
		    (dmultp->ds_outcnt >= dmultp->ds_waitcnt)) {
			mutex_exit(&dmultp->ds_mutex);
			return (0);
		}
		dmultp->ds_outcnt++;
		bp = dmultp->ds_bp;
		dmultp->ds_bp = QUE_DEL(que_objp);

		if (dmultp->ds_kstat)
			kstat_waitq_to_runq(KSTAT_IO_PTR(dmultp->ds_kstat));

		mutex_exit(&dmultp->ds_mutex);

		TGCOM_TRANSPORT(tgcom_objp, bp);

		if (!mutex_tryenter(&dmultp->ds_mutex))
			return (0);
	}
}

static int
dmult_restart(struct fc_data *dmultp)
{
	(void) dmult_deque(dmultp, NULL);
	return (-1);
}

/*
 *	Duplexed Commands per Device: Read Queue and Write Queue
 */
/*
 * Local Function Prototypes
 */
static int duplx_restart();

static int duplx_init(struct duplx_data *fcdp, opaque_t tgcom_objp,
	opaque_t que_objp, void *lkarg);
static int duplx_free(struct flc_obj *flcobjp);
static int duplx_enque();
static int duplx_deque();

struct 	flc_objops duplx_ops = {
	duplx_init,
	duplx_free,
	duplx_enque,
	duplx_deque,
	fc_start_kstat,
	fc_stop_kstat,
	0, 0
};

static opaque_t
duplx_create()
{
	register struct	flc_obj *flcobjp;
	register struct	duplx_data *fcdp;

	flcobjp = (struct flc_obj *)kmem_zalloc((sizeof (*flcobjp) +
		sizeof (*fcdp)), KM_NOSLEEP);
	if (!flcobjp)
		return (NULL);

	fcdp = (struct duplx_data *)(flcobjp+1);
	flcobjp->flc_data = (opaque_t)fcdp;
	flcobjp->flc_ops  = &duplx_ops;

	if (!(fcdp->ds_writeq.fc_qobjp = objmgr_create_obj("qfifo"))) {
		kmem_free(flcobjp, (sizeof (*flcobjp) + sizeof (*fcdp)));
		return (NULL);
	}
	return ((opaque_t)flcobjp);
}

static int
duplx_free(struct flc_obj *flcobjp)
{
	register struct duplx_data *fcdp;

	fcdp = (struct duplx_data *)flcobjp->flc_data;
	if (fcdp->ds_writeq.fc_qobjp) {
		QUE_FREE(fcdp->ds_writeq.fc_qobjp);
		(void) objmgr_destroy_obj("qfifo");
	}
	if (fcdp->ds_readq.fc_qobjp)
		QUE_FREE(fcdp->ds_readq.fc_qobjp);
	if (fcdp->ds_tgcomobjp) {
		TGCOM_FREE(fcdp->ds_tgcomobjp);
		mutex_destroy(&fcdp->ds_mutex);
	}
	kmem_free(flcobjp, (sizeof (*flcobjp) + sizeof (*fcdp)));
	return (0);
}

static int
duplx_init(register struct duplx_data *fcdp, opaque_t tgcom_objp,
	opaque_t que_objp, void *lkarg)
{
	fcdp->ds_tgcomobjp = tgcom_objp;
	fcdp->ds_readq.fc_qobjp = que_objp;

	QUE_INIT(que_objp, lkarg);
	QUE_INIT(fcdp->ds_writeq.fc_qobjp, lkarg);
	TGCOM_INIT(tgcom_objp);

	mutex_init(&fcdp->ds_mutex, NULL, MUTEX_DRIVER, lkarg);

	fcdp->ds_writeq.fc_maxcnt = DUPLX_MAXCNT;
	fcdp->ds_readq.fc_maxcnt  = DUPLX_MAXCNT;

/* 	queues point to each other for round robin 			*/
	fcdp->ds_readq.next = &fcdp->ds_writeq;
	fcdp->ds_writeq.next = &fcdp->ds_readq;

	return (DDI_SUCCESS);
}

static int
duplx_enque(register struct duplx_data *duplxp, struct buf *in_bp)
{
	register opaque_t tgcom_objp;
	register struct fc_que *activeq;
	struct buf *bp;

	mutex_enter(&duplxp->ds_mutex);
	if (in_bp) {
		if (duplxp->ds_kstat) {
			kstat_waitq_enter(KSTAT_IO_PTR(duplxp->ds_kstat));
		}
		if (in_bp->b_flags & B_READ)
			activeq = &duplxp->ds_readq;
		else
			activeq = &duplxp->ds_writeq;

		QUE_ADD(activeq->fc_qobjp, in_bp);
	} else
		activeq = &duplxp->ds_readq;

	tgcom_objp = duplxp->ds_tgcomobjp;

	for (;;) {
		if (!activeq->fc_bp)
			activeq->fc_bp = QUE_DEL(activeq->fc_qobjp);
		if (!activeq->fc_bp ||
		    (TGCOM_PKT(tgcom_objp, activeq->fc_bp, duplx_restart,
		    (caddr_t)duplxp) != DDI_SUCCESS) ||
		    (activeq->fc_outcnt >= activeq->fc_maxcnt)) {

			/* switch read/write queues */
			activeq = activeq->next;
			if (!activeq->fc_bp)
				activeq->fc_bp = QUE_DEL(activeq->fc_qobjp);
			if (!activeq->fc_bp ||
			    (TGCOM_PKT(tgcom_objp, activeq->fc_bp,
			    duplx_restart, (caddr_t)duplxp) != DDI_SUCCESS) ||
			    (activeq->fc_outcnt >= activeq->fc_maxcnt)) {
				mutex_exit(&duplxp->ds_mutex);
				return (0);
			}
		}

		activeq->fc_outcnt++;
		bp = activeq->fc_bp;
		activeq->fc_bp = NULL;

		if (duplxp->ds_kstat)
			kstat_waitq_to_runq(KSTAT_IO_PTR(duplxp->ds_kstat));
		mutex_exit(&duplxp->ds_mutex);

		TGCOM_TRANSPORT(tgcom_objp, bp);

		if (!mutex_tryenter(&duplxp->ds_mutex))
			return (0);

		activeq = activeq->next;
	}
}

static int
duplx_deque(register struct duplx_data *duplxp, struct buf *in_bp)
{
	register opaque_t tgcom_objp;
	register struct fc_que *activeq;
	struct buf *bp;

	mutex_enter(&duplxp->ds_mutex);

	tgcom_objp = duplxp->ds_tgcomobjp;

	if (in_bp->b_flags & B_READ)
		activeq = &duplxp->ds_readq;
	else
		activeq = &duplxp->ds_writeq;
	activeq->fc_outcnt--;

	if (duplxp->ds_kstat) {
		if (in_bp->b_flags & B_READ) {
			KSTAT_IO_PTR(duplxp->ds_kstat)->reads++;
			KSTAT_IO_PTR(duplxp->ds_kstat)->nread +=
				(in_bp->b_bcount - in_bp->b_resid);
		} else {
			KSTAT_IO_PTR(duplxp->ds_kstat)->writes++;
			KSTAT_IO_PTR(duplxp->ds_kstat)->nwritten +=
				(in_bp->b_bcount - in_bp->b_resid);
		}
		kstat_runq_exit(KSTAT_IO_PTR(duplxp->ds_kstat));
	}

	for (;;) {

		/* if needed, try to pull request off a queue */
		if (!activeq->fc_bp)
			activeq->fc_bp = QUE_DEL(activeq->fc_qobjp);

		if (!activeq->fc_bp ||
		    (TGCOM_PKT(tgcom_objp, activeq->fc_bp, duplx_restart,
		    (caddr_t)duplxp) != DDI_SUCCESS) ||
		    (activeq->fc_outcnt >= activeq->fc_maxcnt)) {

			activeq = activeq->next;
			if (!activeq->fc_bp)
				activeq->fc_bp = QUE_DEL(activeq->fc_qobjp);

			if (!activeq->fc_bp ||
			    (TGCOM_PKT(tgcom_objp, activeq->fc_bp,
			    duplx_restart, (caddr_t)duplxp) != DDI_SUCCESS) ||
			    (activeq->fc_outcnt >= activeq->fc_maxcnt)) {
				mutex_exit(&duplxp->ds_mutex);
				return (0);
			}
		}

		activeq->fc_outcnt++;
		bp = activeq->fc_bp;
		activeq->fc_bp = NULL;

		if (duplxp->ds_kstat)
			kstat_waitq_to_runq(KSTAT_IO_PTR(duplxp->ds_kstat));

		mutex_exit(&duplxp->ds_mutex);

		TGCOM_TRANSPORT(tgcom_objp, bp);

		if (!mutex_tryenter(&duplxp->ds_mutex))
			return (0);

		activeq = activeq->next;
	}
}

static int
duplx_restart(struct duplx_data *duplxp)
{
	(void) duplx_enque(duplxp, NULL);
	return (-1);
}

/*
 *	Tagged queueing flow control
 */
/*
 * Local Function Prototypes
 */

struct 	flc_objops adapt_ops = {
	fc_init,
	fc_free,
	dmult_enque,
	dmult_deque,
	fc_start_kstat,
	fc_stop_kstat,
	0, 0
};

static opaque_t
adapt_create()
{
	return (fc_create((struct flc_objops *)&adapt_ops));

}

/*
 *	Common Queue functions
 */

/*
 * 	Local static data
 */
#ifdef	Q_DEBUG
#define	DENT	0x0001
#define	DERR	0x0002
#define	DIO	0x0004
static	int	que_debug = DENT|DERR|DIO;

#endif	/* Q_DEBUG */
/*
 * 	Local Function Prototypes
 */
static opaque_t que_create(struct que_objops *qopsp);
static int que_init(struct que_data *qfp, void *lkarg);
static int que_free(struct que_obj *queobjp);
static struct buf *que_del(struct que_data *qfp);

static opaque_t
que_create(register struct que_objops *qopsp)
{
	register struct	que_data *qfp;
	register struct	que_obj *queobjp;

	queobjp = (struct que_obj *)kmem_zalloc((sizeof (*queobjp) +
			sizeof (*qfp)), KM_NOSLEEP);
	if (!queobjp)
		return (NULL);

	queobjp->que_ops = qopsp;
	qfp = (struct que_data *)(queobjp+1);
	queobjp->que_data = (opaque_t)qfp;

	return ((opaque_t)queobjp);
}

static int
que_init(register struct que_data *qfp, register void *lkarg)
{
	mutex_init(&qfp->q_mutex, NULL, MUTEX_DRIVER, lkarg);
	return (DDI_SUCCESS);
}

static int
que_free(register struct que_obj *queobjp)
{
	register struct	que_data *qfp;

	qfp = (struct que_data *)queobjp->que_data;
	mutex_destroy(&qfp->q_mutex);
	kmem_free(queobjp, (sizeof (*queobjp) + sizeof (struct que_data)));
	return (0);
}

static struct buf *
que_del(register struct que_data *qfp)
{
	register struct buf *bp;

	bp = qfp->q_tab.b_actf;
	if (bp) {
		qfp->q_tab.b_actf = bp->av_forw;
		if (!qfp->q_tab.b_actf)
			qfp->q_tab.b_actl = NULL;
		bp->av_forw = 0;
	}
	return (bp);
}



/*
 *	Qmerge
 * 	Local Function Prototypes
 */
static int qmerge_add(), qmerge_free();
static struct buf *qmerge_del(struct que_data *qfp);

struct 	que_objops qmerge_ops = {
	que_init,
	qmerge_free,
	qmerge_add,
	qmerge_del,
	0, 0
};

/* fields in diskhd */
#define	hd_cnt			b_back
#define	hd_private		b_forw
#define	hd_flags		b_flags
#define	hd_sync_next		av_forw
#define	hd_async_next		av_back

#define	hd_sync2async		sync_async_ratio

#define	QNEAR_FORWARD		0x01
#define	QNEAR_BACKWARD		0x02
#define	QNEAR_ASYNCONLY		0x04
#define	QNEAR_ASYNCALSO		0x08

#define	BP_LT_BP(a, b)	((u_int)((a)->b_private) < (u_int)((b)->b_private))
#define	BP_GT_BP(a, b)	((u_int)((a)->b_private) > (u_int)((b)->b_private))
#define	BP_LT_HD(a, b)	((u_int)((a)->b_private) < (u_int)((b)->hd_private))
#define	BP_GT_HD(a, b)	((u_int)((a)->b_private) > (u_int)((b)->hd_private))
#define	QNEAR_ASYNC	(QNEAR_ASYNCONLY|QNEAR_ASYNCALSO)

#define	SYNC2ASYNC(a) ((a)->q_tab.hd_cnt)


/*
 * qmerge implements a two priority queue, the low priority queue holding ASYNC
 * write requests, while the rest are queued in the high priority sync queue.
 * Requests on the async queue would be merged if possible.
 * By default qmerge2wayscan is 1, indicating an elevator algorithm. When
 * this variable is set to zero, it has the following side effects.
 * 1. We assume fairness is the number one issue.
 * 2. The next request to be picked indicates current head position.
 *
 * qmerge_sync2async indicates the ratio of scans of high prioriy
 * sync queue to low priority async queue.
 *
 *
 *
 * When qmerge variables have the following values it defaults to qsort
 *
 * qmerge1pri = 1, qmerge2wayscan = 0, qmerge_max_merge = 0
 *
 */
int	qmerge_max_merge = 128 * 1024;
int	qmerge_sync2async = 4;
int	qmerge2wayscan = 1;
int	qmerge1pri = 0;
int	qmerge_merge = 0;


/* fields in bp */

/*
 * 	Local static data
 */
static opaque_t
qmerge_create()
{
	register struct	que_data *qfp;
	register struct	que_obj *queobjp;

	queobjp = (struct que_obj *)kmem_zalloc((sizeof (*queobjp) +
			sizeof (*qfp)), KM_NOSLEEP);
	if (!queobjp)
		return (NULL);

	queobjp->que_ops = &qmerge_ops;
	qfp = (struct que_data *)(queobjp+1);
	qfp->q_tab.hd_private = qfp->q_tab.hd_private = 0;
	qfp->q_tab.hd_sync_next = qfp->q_tab.hd_async_next = NULL;
	qfp->q_tab.hd_cnt = (void *)qmerge_sync2async;
	queobjp->que_data = (opaque_t)qfp;

	return ((opaque_t)queobjp);
}

static int
qmerge_free(register struct que_obj *queobjp)
{
	register struct	que_data *qfp;

	qfp = (struct que_data *)queobjp->que_data;
	mutex_destroy(&qfp->q_mutex);
	kmem_free(queobjp, (sizeof (*queobjp) + sizeof (*qfp)));
	return (0);
}



static int
qmerge_can_merge(bp1, bp2)
struct	buf *bp1, *bp2;
{


	if ((bp1->b_un.b_addr != 0) || (bp2->b_un.b_addr != 0)		||
	((bp1->b_flags & (B_PAGEIO|B_REMAPPED|B_ASYNC|B_WRITE)) !=
			(B_PAGEIO|B_ASYNC|B_WRITE))			||
	((bp2->b_flags & (B_PAGEIO|B_REMAPPED|B_ASYNC|B_WRITE)) !=
			(B_PAGEIO|B_ASYNC|B_WRITE))			||
	(bp1->b_bcount & PAGEOFFSET) || (bp2->b_bcount & PAGEOFFSET)	||
	(bp1->b_bcount + bp2->b_bcount > qmerge_max_merge))
		return (0);

	if (((u_int)bp2->b_private + bp2->b_bcount/DEV_BSIZE 	==
		(u_int)bp1->b_private) 				||
		((u_int)bp1->b_private + bp1->b_bcount/DEV_BSIZE ==
		(u_int)bp2->b_private))

		return (1);
	else
		return (0);
}

static void
qmerge_mergesetup(bp_merge, bp)
struct	buf *bp_merge, *bp;
{
	struct	buf *bp1;
	struct	page *pp, *pp_merge, *pp_merge_prev;
	int	forward;

	qmerge_merge++;
	forward = bp_merge->b_private < bp->b_private;

	bp_merge->b_bcount += bp->b_bcount;

	pp = bp->b_pages;
	pp_merge = bp_merge->b_pages;

	pp_merge_prev = pp_merge->p_prev;

	pp_merge->p_prev->p_next = pp;
	pp_merge->p_prev = pp->p_prev;
	pp->p_prev->p_next = pp_merge;
	pp->p_prev = pp_merge_prev;

	bp1 = bp_merge->b_forw;

	bp1->av_back->av_forw = bp;
	bp->av_back = bp1->av_back;
	bp1->av_back = bp;
	bp->av_forw = bp1;

	if (!forward) {
		bp_merge->b_forw = bp;
		bp_merge->b_pages = pp;
		bp_merge->b_private = bp->b_private;
	}
}

static void
que_insert(register struct que_data *qfp, register struct buf *bp)
{
	struct	buf *bp1, *bp_start, *lowest_bp, *highest_bp;
	unsigned long	highest_blk, lowest_blk;
	struct	buf **async_bpp, **sync_bpp, **bpp;
	struct	diskhd	*dp = &qfp->q_tab;


	sync_bpp = &dp->hd_sync_next;
	async_bpp = &dp->hd_async_next;
	/*
	 * The ioctl used by the format utility requires that bp->av_back be
	 * preserved.
	 */
	if (bp->av_back)
		bp->b_error = (int)bp->av_back;
	if (!qmerge1pri &&
		((bp->b_flags & (B_ASYNC|B_READ|B_FREE)) == B_ASYNC)) {
		bpp = &dp->hd_async_next;
	} else {
		bpp = &dp->hd_sync_next;
	}


	if ((bp1 = *bpp) == NULL) {
		*bpp = bp;
		bp->av_forw = bp->av_back = bp;
		if ((bpp == async_bpp) && (*sync_bpp == NULL))
			dp->hd_flags |= QNEAR_ASYNCONLY;
		else if (bpp == sync_bpp) {
			dp->hd_flags &= ~QNEAR_ASYNCONLY;
			if (*async_bpp)
				dp->hd_flags |= QNEAR_ASYNCALSO;
		}
		return;
	}
	bp_start = bp1;
	if (bp->b_private < bp1->b_private) {
		lowest_blk = (unsigned long)bp1->b_private;
		lowest_bp = bp1;
		do {
			if (bp->b_private > bp1->b_private) {
				bp->av_forw = bp1->av_forw;
				bp1->av_forw->av_back = bp;
				bp1->av_forw = bp;
				bp->av_back = bp1;


				if (((bpp == async_bpp) &&
				    (dp->hd_flags & QNEAR_ASYNC)) ||
				    (bpp == sync_bpp)) {
					if (!(dp->hd_flags & QNEAR_BACKWARD) &&
						BP_GT_HD(bp, dp)) {
						*bpp = bp;
					}
			}
				return;
			} else if ((unsigned long)bp1->b_private < lowest_blk) {
				lowest_bp = bp1;
				lowest_blk = (unsigned long)bp1->b_private;
			}
		} while ((bp1->av_back->b_private < bp1->b_private) &&
			((bp1 = bp1->av_back) != bp_start));
		bp->av_forw = lowest_bp;
		lowest_bp->av_back->av_forw = bp;
		bp->av_back = lowest_bp->av_back;
		lowest_bp->av_back = bp;
		if ((bpp == async_bpp) && !(dp->hd_flags & QNEAR_ASYNC))
			*bpp = bp;
		else if (!(dp->hd_flags & QNEAR_BACKWARD) && BP_GT_HD(bp, dp)) {
			*bpp = bp;
		}
	} else {

		highest_blk = (unsigned long)bp1->b_private;
		highest_bp = bp1;
		do {
			if (bp->b_private < bp1->b_private) {
				bp->av_forw = bp1;
				bp1->av_back->av_forw = bp;
				bp->av_back = bp1->av_back;
				bp1->av_back = bp;
				if (((bpp == async_bpp) &&
				(dp->hd_flags & QNEAR_ASYNC)) ||
				(bpp == sync_bpp)) {
					if ((dp->hd_flags & QNEAR_BACKWARD) &&
						BP_LT_HD(bp, dp)) {
						*bpp = bp;
					}
				}
				return;
			} else if ((unsigned long)bp1->b_private >
					highest_blk) {
				highest_bp = bp1;
				highest_blk = (unsigned long)bp1->b_private;
			}
		} while ((bp1->av_forw->b_private > bp1->b_private) &&
			((bp1 = bp1->av_forw) != bp_start));
		bp->av_back = highest_bp;
		highest_bp->av_forw->av_back  = bp;
		bp->av_forw = highest_bp->av_forw;
		highest_bp->av_forw = bp;

		if (((bpp == sync_bpp) ||
		((bpp == async_bpp) && (dp->hd_flags & QNEAR_ASYNC))) &&
		(dp->hd_flags & QNEAR_BACKWARD) && (BP_LT_HD(bp, dp)))
			*bpp = bp;
	}
}

/*
 * dmult_enque() holds dmultp->ds_mutex lock, so we dont grab
 * lock here. If dmult_enque() changes we will have to visit
 * this function again
 */
static int
qmerge_add(register struct que_data *qfp, register struct buf *bp)
{

	que_insert(qfp, bp);
	return (++qfp->q_cnt);
}

static void
qmerge_iodone(bp)
struct	buf *bp;
{
	struct buf *bp1;
	struct	page *pp, *pp1, *tmp_pp;

	if (bp->b_flags & B_REMAPPED)
		bp_mapout(bp);

	bp1 = bp->b_forw;
	do {
		bp->b_forw = bp1->av_forw;
		bp1->av_forw->av_back = bp1->av_back;
		bp1->av_back->av_forw = bp1->av_forw;
		pp = (page_t *)bp1->b_pages;
		pp1 = bp->b_forw->b_pages;

		tmp_pp = pp->p_prev;
		pp->p_prev = pp1->p_prev;
		pp->p_prev->p_next = pp;

		pp1->p_prev = tmp_pp;
		pp1->p_prev->p_next = pp1;

		if (bp->b_flags & B_ERROR) {
			bp1->b_error = bp->b_error;
			bp1->b_oerror = bp->b_oerror;
			bp1->b_flags |= B_ERROR;
		}

		biodone(bp1);
	} while ((bp1 = bp->b_forw) != bp->b_forw->av_forw);

	biodone(bp1);
	kmem_free(bp, sizeof (*bp));
}




static struct buf *
qmerge_nextbp(struct que_data *qfp, struct buf *bp_merge, int *can_merge)
{
	int	private, flags, cnt;
	struct	buf *sync_bp, *async_bp, *bp;
	struct	buf **sync_bpp, **async_bpp, **bpp;
	struct	diskhd *dp = &qfp->q_tab;

	if (qfp->q_cnt == 0) {
		return (NULL);
	}
	flags = qfp->q_tab.hd_flags;
	sync_bpp = &qfp->q_tab.hd_sync_next;
	async_bpp = &qfp->q_tab.hd_async_next;



begin_nextbp:
	if (flags & QNEAR_ASYNCONLY) {
		bp = *async_bpp;
		private = (int)bp->b_private;
		if (bp_merge && !qmerge_can_merge(bp, bp_merge)) {
			return (NULL);
		} else if (bp->av_forw == bp) {
			bp->av_forw = bp->av_back = NULL;
			flags &= ~(QNEAR_ASYNCONLY | QNEAR_BACKWARD);
			private = 0;
		} else if (flags & QNEAR_BACKWARD) {
			if (bp->b_private < bp->av_back->b_private) {
				flags &= ~QNEAR_BACKWARD;
				private = 0;
			}
		} else if (bp->b_private > bp->av_forw->b_private) {
			if (qmerge2wayscan) {
				flags |= QNEAR_BACKWARD;
			} else
				private = 0;
		} else if (qmerge2wayscan == 0) {
			private = (int)bp->av_forw->b_private;
		}
		bpp = async_bpp;

	} else if (flags & QNEAR_ASYNCALSO) {
		sync_bp = *sync_bpp;
		async_bp = *async_bpp;
		if (flags & QNEAR_BACKWARD) {
			if (BP_GT_HD(sync_bp, dp) && BP_GT_HD(async_bp, dp)) {
				flags &= ~(QNEAR_BACKWARD|QNEAR_ASYNCALSO);
				*sync_bpp = sync_bp->av_forw;
				*async_bpp = async_bp->av_forw;
				SYNC2ASYNC(qfp) = (void *)qmerge_sync2async;
				qfp->q_tab.hd_private = 0;
				goto begin_nextbp;
			}
			if (BP_LT_HD(async_bp, dp) && BP_LT_HD(sync_bp, dp)) {
				if (BP_GT_BP(async_bp, sync_bp)) {
					bpp = async_bpp;
					bp = *async_bpp;
				} else {
					bpp = sync_bpp;
					bp = *sync_bpp;
				}
			} else if (BP_LT_HD(async_bp, dp)) {
				bpp = async_bpp;
				bp = *async_bpp;
			} else {
				bpp = sync_bpp;
				bp = *sync_bpp;
			}
		} else {
			if (BP_LT_HD(sync_bp, dp) && BP_LT_HD(async_bp, dp)) {
				if (qmerge2wayscan) {
					flags |= QNEAR_BACKWARD;
					*sync_bpp = sync_bp->av_back;
					*async_bpp = async_bp->av_back;
					goto begin_nextbp;
				} else {
					flags &= ~QNEAR_ASYNCALSO;
					SYNC2ASYNC(qfp) =
						(void *)qmerge_sync2async;
					qfp->q_tab.hd_private = 0;
					goto begin_nextbp;
				}
			}
			if (BP_GT_HD(async_bp, dp) && BP_GT_HD(sync_bp, dp)) {
				if (BP_LT_BP(async_bp, sync_bp)) {
					bpp = async_bpp;
					bp = *async_bpp;
				} else {
					bpp = sync_bpp;
					bp = *sync_bpp;
				}
			} else if (BP_GT_HD(async_bp, dp)) {
				bpp = async_bpp;
				bp = *async_bpp;
			} else {
				bpp = sync_bpp;
				bp = *sync_bpp;
			}
		}
		if (bp_merge && !qmerge_can_merge(bp, bp_merge))
			return (NULL);
		else if (bp->av_forw == bp) {
			bp->av_forw = bp->av_back = NULL;
			flags &= ~QNEAR_ASYNCALSO;
			if (bpp == async_bpp) {
				SYNC2ASYNC(qfp) = (void *)qmerge_sync2async;
			}
			else
				flags |= QNEAR_ASYNCONLY;
		}
		private = (int)bp->b_private;
	} else {
		bp = *sync_bpp;
		private = (int)bp->b_private;
		if (bp_merge && !qmerge_can_merge(bp, bp_merge)) {
			return (NULL);
		} else if (bp->av_forw == bp) {
			private = 0;
			SYNC2ASYNC(qfp) = (void *)qmerge_sync2async;
			bp->av_forw = bp->av_back = NULL;
			flags &= ~QNEAR_BACKWARD;
			if (*async_bpp)
				flags |= QNEAR_ASYNCONLY;
		} else if (flags & QNEAR_BACKWARD) {
			if (bp->b_private < bp->av_back->b_private) {
				flags &= ~QNEAR_BACKWARD;
				if ((cnt = (int)SYNC2ASYNC(qfp)) > 0) {
					cnt--;
					SYNC2ASYNC(qfp) = (void *)cnt;
				} else {
					if (*async_bpp)
						flags |= QNEAR_ASYNCALSO;
					SYNC2ASYNC(qfp) =
						(void *)qmerge_sync2async;
				}
				private = 0;
			}
		} else if (bp->b_private > bp->av_forw->b_private) {
			private = 0;
			if (qmerge2wayscan) {
				flags |= QNEAR_BACKWARD;
				private = (int)bp->b_private;
			} else {
				if ((cnt = (int)SYNC2ASYNC(qfp)) > 0) {
					cnt--;
					SYNC2ASYNC(qfp) = (void *)cnt;
				} else {
					if (*async_bpp)
						flags |= QNEAR_ASYNCALSO;
					SYNC2ASYNC(qfp) =
						(void *)qmerge_sync2async;
				}
			}
		} else if (qmerge2wayscan == 0) {
			private = (int)bp->av_forw->b_private;
		}
		bpp = sync_bpp;

	}

	if (bp->av_forw) {
		if (flags & QNEAR_BACKWARD) {
			*bpp = bp->av_back;
			*can_merge = ((((u_int)bp->av_back->b_private +
			bp->av_back->b_bcount/DEV_BSIZE) ==
			(u_int)bp->b_private) &&
			!(bp->b_flags & B_READ));
		} else {
			*bpp = bp->av_forw;
			*can_merge = ((((u_int)bp->b_private +
				bp->b_bcount/DEV_BSIZE)
				== (u_int)bp->av_forw->b_private) &&
				!(bp->b_flags & B_READ));
		}
		bp->av_forw->av_back = bp->av_back;
		bp->av_back->av_forw = bp->av_forw;
		bp->av_forw = bp->av_back = NULL;
	} else {
		*bpp = NULL;
		*can_merge = 0;
	}
	qfp->q_tab.hd_private = (void *)private;
	qfp->q_cnt--;
	qfp->q_tab.hd_flags = flags;
	if (bp->b_error) {
		bp->av_back = (void *)bp->b_error;
		bp->b_error = 0;
	}
	return (bp);
}

static struct buf *
qmerge_del(register struct que_data *qfp)
{
	struct	buf *bp, *next_bp, *bp_merge;
	int	alloc_mergebp, merge;

	if (qfp->q_cnt == 0) {
		return (NULL);
	}

	bp_merge = bp = qmerge_nextbp(qfp, NULL, &merge);
	alloc_mergebp = 1;
	while (merge && (next_bp = qmerge_nextbp(qfp, bp_merge, &merge))) {
		if (alloc_mergebp) {
			bp_merge = (struct buf *)
				kmem_alloc(sizeof (*bp_merge), KM_NOSLEEP);
			if (bp_merge == NULL) {
				mutex_exit(&qfp->q_mutex);
				return (bp);
			}
			bcopy((caddr_t)bp, (caddr_t)bp_merge,
				sizeof (*bp_merge));
			bp_merge->b_iodone = (int (*)())qmerge_iodone;
			bp_merge->b_forw = bp;
			bp_merge->b_back = (struct buf *)qfp;
			bp->av_forw = bp->av_back = bp;
			alloc_mergebp = 0;
		}
		qmerge_mergesetup(bp_merge, next_bp);
	}
	return (bp_merge);
}


/*
 *	FIFO Queue functions
 */
/*
 * 	Local Function Prototypes
 */
static int qfifo_add();

struct 	que_objops qfifo_ops = {
	que_init,
	que_free,
	qfifo_add,
	que_del,
	0, 0
};

/*
 * 	Local static data
 */
static opaque_t
qfifo_create()
{
	return (que_create((struct que_objops *)&qfifo_ops));
}

static int
qfifo_add(register struct que_data *qfp, register struct buf *bp)
{

	if (!qfp->q_tab.b_actf)
		qfp->q_tab.b_actf = bp;
	else
		qfp->q_tab.b_actl->av_forw = bp;
	qfp->q_tab.b_actl = bp;
	bp->av_forw = NULL;
	return (0);
}

/*
 *	One-Way-Scan Queue functions
 */
/*
 * 	Local Function Prototypes
 */
static int qsort_add();
static struct buf *qsort_del();
static void oneway_scan_binary(struct diskhd *dp, struct buf *bp);

struct 	que_objops qsort_ops = {
	que_init,
	que_free,
	qsort_add,
	qsort_del,
	0, 0
};

/*
 * 	Local static data
 */
static opaque_t
qsort_create()
{
	return (que_create((struct que_objops *)&qsort_ops));
}

static int
qsort_add(register struct que_data *qfp, register struct buf *bp)
{
	qfp->q_cnt++;
	oneway_scan_binary(&qfp->q_tab, bp);
	return (0);
}


#define	b_pasf	b_forw
#define	b_pasl	b_back
static void
oneway_scan_binary(register struct diskhd *dp, register struct buf *bp)
{
	register struct buf *ap;

	ap = dp->b_actf;
	if (ap == NULL) {
		dp->b_actf = bp;
		bp->av_forw = NULL;
		return;
	}
	if ((daddr_t)bp->b_private < (daddr_t)ap->b_private) {
		ap = dp->b_pasf;
		if ((ap == NULL) || ((daddr_t)bp->b_private <
			(daddr_t)ap->b_private)) {
			dp->b_pasf = bp;
			bp->av_forw = ap;
			return;
		}
	}
	while (ap->av_forw) {
		if ((daddr_t)bp->b_private < (daddr_t)ap->av_forw->b_private)
			break;
		ap = ap->av_forw;
	}
	bp->av_forw = ap->av_forw;
	ap->av_forw = bp;
}

static struct buf *
qsort_del(register struct que_data *qfp)
{
	register struct buf *bp;

	if (qfp->q_cnt == 0) {
		return (NULL);
	}
	qfp->q_cnt--;
	bp = qfp->q_tab.b_actf;
	qfp->q_tab.b_actf = bp->av_forw;
	bp->av_forw = 0;
	if (!qfp->q_tab.b_actf && qfp->q_tab.b_pasf) {
		qfp->q_tab.b_actf = qfp->q_tab.b_pasf;
		qfp->q_tab.b_pasf = NULL;
	}
	return (bp);
}

/*
 *	Tagged queueing
 */
/*
 * 	Local Function Prototypes
 */

struct 	que_objops qtag_ops = {
	que_init,
	que_free,
	qsort_add,
	qsort_del,
	0, 0
};

/*
 * 	Local static data
 */
static opaque_t
qtag_create()
{
	return (que_create((struct que_objops *)&qtag_ops));
}
