/*****************************************************************************
 * Copyright (c) 1988-95 Digi International Inc., All Rights Reserved
 * 
 * This  software  contains  proprietary  and  confidential   information   of
 * Digi International  Inc.   By  accepting  transfer  of this copy, Recipient
 * agrees to retain this software in  confidence,  to  prevent  disclosure  to
 * others,  and  to make no  use of this software other than that for which it
 * was delivered.  This is an unpublished copyright work  Digi   International
 * Inc.    Except   as   permitted   by  federal  law,  17 USC 117, copying is
 * strictly prohibited.
 * 
 * Digi  International  Inc.  CONFIDENTIAL  -   (Digi    International    Inc.
 * Confidential  when   combined   with   the   aggregated  modules  for  this
 * product) OBJECT CODE ONLY SOURCE MATERIALS
 * 
 * (C) COPYRIGHT Digi International Inc. 1995
 * 
 * All Rights Reserved
 * 
 * US   Government   Users   Restricted   Rights   -   Use,   duplication   or
 * disclosure  restricted by GSA ADP Schedule Contract with Digi International
 * Inc.
 * 
 *****************************************************************************/

/******************************************************************
 *
 *  Embedded STREAMS Package
 *
 *  Copyright 1990  Spider Systems Limited
 *
 *  QUEUE.C
 *
 *    Queue management
 *
 ******************************************************************/

/*
 * $Id: queue.c,v 1.8 1997/10/22 21:52:24 gary Exp $
 */


#include "types.h"
#include "stream.h"

#if SOH
#include "sohalloc.h"
#else
#include "alloc.h"
#endif

#include "cnt_arry.h" 
#include "queue.h" 

queue_t *runqhead, *runqtail;
int needsrunq;

/************************/
/*	initq		*/
/************************/
void
initq(queue_t *q, struct qinit *rinit, struct qinit *winit)
{
    struct module_info *minfo;

    /* must be write side */
    if (q->q_flag & QREADR) q = WR(q);

    /* Fill in write side */
    q->q_qinfo = winit;
    q->q_ptr = (caddr_t)0;
    if (winit->qi_srvp)
    {
	q->q_flag |= QSERV;
	q->q_can = q;
    }
    else
    {
	q->q_flag &= ~QSERV;
	if (q->q_next) q->q_can = q->q_next->q_can;
    }

    q->q_flag |= QSCHED;
    minfo = winit->qi_minfo;
    q->q_minpsz = minfo->mi_minpsz;
    q->q_maxpsz = minfo->mi_maxpsz;
    q->q_hiwat = minfo->mi_hiwat;
    q->q_lowat = minfo->mi_lowat;

    /* Now fill in read side */
    q = RD(q);
    q->q_qinfo = rinit;
    q->q_ptr = (caddr_t)0;
    if (rinit->qi_srvp)
    {
	q->q_flag |= QSERV;
	q->q_can = q;
    }
    else
    {
	q->q_flag &= ~QSERV;
    }
    q->q_flag |= QSCHED;
    minfo = rinit->qi_minfo;
    q->q_minpsz = minfo->mi_minpsz;
    q->q_maxpsz = minfo->mi_maxpsz;
    q->q_hiwat = minfo->mi_hiwat;
    q->q_lowat = minfo->mi_lowat;
}


/************************/
/*	linkq		*/
/************************/
void
linkq(queue_t *hq, queue_t *q)
{
    /*
     * Link new QUEUE pair q (which must be already initialised)
     * under stream head hq, where q and hq are
     * the write queues of their QUEUE pairs.
     */

    queue_t *xq, *nq = hq->q_next;

    /* must be write sides */
    if (hq->q_flag & QREADR) hq = WR(hq);
    if (q->q_flag & QREADR) q = WR(q);

    /* Connect new queue to head queue */
    hq->q_next = q;
    RD(q)->q_next = RD(hq);

    /* If module, connect tail of stream to it */
    if (q->q_next = nq)
    {   /* Pushing a module */
	RD(nq)->q_next = RD(q);

	/* Penultimate queue q_back always points to end */
	q->q_back = hq;

	/* Fix up write side flow control pointers q_can and q_back */
	if (q->q_flag & (QSERV|QEND))
	{   /* Write side of q has a service function or is driver */
	    for (xq = nq;; xq = xq->q_next)
	    {
		xq->q_back = q;
		if (xq->q_flag & (QSERV|QEND)) break;
	    }
	}
	else
	{   /* Module with no write service function */
	    q->q_can = nq->q_can;
	}

	/* Fix up read side flow control pointers q_can and q_back */
	if (RD(q)->q_flag & QSERV)
	{   /* Read side of q has a service function. */
	    RD(q)->q_back = RD(hq)->q_back;
	    RD(hq)->q_back = RD(q);

	    /* Fill in read q_can pointers */
	    for (xq = q; xq && (RD(xq)->q_flag & QSERV)==0; xq = xq->q_next)
		RD(xq)->q_can = RD(q);
	}
	else
	{   /* No service function */
	    RD(q)->q_can = RD(hq);
	}
    }
    else
    {   /* Attaching the driver */
	q->q_flag |= QEND;

	/* Fix up head write queue */
	if ((hq->q_flag & QSERV)==0)
	{   /* Head write side has no service function (yet) */
	    hq->q_can = q;
	}
	q->q_back = hq;

	/* Fix up head read queue */
	RD(hq)->q_flag |= QEND;
	RD(hq)->q_can = RD(hq);
	RD(hq)->q_back = RD(q);
    }
}


/************************/
/*	unlinkq		*/
/************************/
void
unlinkq(queue_t *q)
{
    /*
     * Unlink module QUEUE pair q, which must be
     * immediately below the QUEUE head
     */

    queue_t *hq, *nq, *xq;

    /* must be write side */
    if (q->q_flag & QREADR) q = WR(q);

    /* Next module/driver down */
    nq = q->q_next;

    /* Stream head queues */
    hq = WR(RD(q)->q_next);

    /* Attach next down to head */
    hq->q_next = nq;

    /* Set head canput pointer */
    if ((hq->q_flag & QSERV)==0)
    {   /* Head write side has no service function */
	hq->q_can = nq ? nq->q_can : (queue_t *)0;
    }

    /* If lower module/driver, reconnect */
    if (nq)
    {
	RD(nq)->q_next = RD(nq);
	for (xq = nq;
	    (xq->q_flag & (QSERV|QEND))==0;
	     xq = xq->q_next);
	xq->q_back = hq;
    }

    if (RD(q)->q_flag & QSERV)
    {   /* Read side of q had a service function. */

	/* Close up the q_back chain */
	RD(hq)->q_back = RD(q)->q_back;

	/* Fill in read q_can pointers back to but not including prev QSERV */
	for (xq = nq; xq && (RD(xq)->q_flag & QSERV)==0; xq = xq->q_next)
	    RD(xq)->q_can = RD(hq);
    }
}


/************************/
/*	backq		*/
/************************/
queue_t *
backq(register queue_t *q)
{
    if (q = OTHERQ(q)->q_next) return OTHERQ(q);
    return (queue_t *)0;
}

/************************/
/*	canput		*/
/************************/
int 
canput(queue_t *q)
{
    if ((q = q->q_can) && (q->q_flag & QFULL))
    {
	q->q_flag |= QBACK;
	return 0;
    }

    return 1;
}

/************************/
/*	flushq		*/
/************************/
void 
flushq(queue_t *q, int flag)
{
    mblk_t *mp;

    if (mp = q->q_first)
    {
	mblk_t *bp;

	mp->b_datap->db_diag = 0x10;
	if (flag==FLUSHALL)
	{
	    do
	    {
		bp = mp;
		mp = mp->b_next;
		bp->b_next = (mblk_t *) 0;
		freemsg(bp);
	    }
	    while (mp);

	    q->q_first = q->q_last = (mblk_t *)0;
	    q->q_flag &= ~QFULL;
	    q->q_count = 0;
	}
	else
	{
	    do
	    {
		int type;

		bp = mp;
		mp = mp->b_next;
		if(mp)
		{
			if ((type = mp->b_datap->db_type)==M_DATA ||
	     			type==M_PROTO || type==M_PCPROTO)
			{
	    			rmvq(q, bp);
	    			freemsg(bp);
			}
		}
	    }
	    while (mp);
	}
    }

    if (q->q_count == 0)
    {
         q->q_flag |= QSCHED;
    }

    if ((q->q_count <= q->q_lowat) && (q->q_flag & QBACK))
    {
        q->q_flag &= ~QBACK;
        if ((q = q->q_back) && (q->q_flag & QSERV))
            qenable(q);
    }


}

/************************/
/*	getq		*/
/************************/
mblk_t *
getq(queue_t *q)
{
    mblk_t *mp, *bp;
    ushort sz = q->q_count;
#ifdef SPLSTR
    int s = splstr();
#endif
    if (mp = q->q_first)
    {
	if (q->q_first = bp = mp->b_next)
	{
	    mp->b_next = (mblk_t *)0;
	    bp->b_prev = (mblk_t *)0;
	    sz = q->q_count;
	    for(bp = mp; bp; bp = bp->b_cont)
		sz -= (int)(bp->b_wptr - bp->b_rptr);
	}
	else
	{
	    q->q_last = (mblk_t *)0;
	    sz = 0;
	}

	/* High-water mark */
	if ((q->q_count = sz) < q->q_hiwat)
	    q->q_flag &= ~QFULL;

	/* Low-water mark */
	if (sz <= q->q_lowat && (q->q_flag & QBACK))
	{
	    q->q_flag &= ~QBACK;
	    if ((q = q->q_back) && (q->q_flag & QSERV))
		qenable(q);
	}
        mp->b_datap->db_diag = 0x11;
    }
    else
	q->q_flag |= QSCHED;

#ifdef SPLSTR
    splx(s);
#endif
    return mp;
}


/************************/
/*	insq		*/
/************************/
void insq(queue_t *q, mblk_t *emp, mblk_t *nmp)
{
    mblk_t *xmp;

    ushort sz;
#ifdef SPLSTR
    int s = splstr();
#endif

    if (emp)
    {   /* Not at end of queue */
	if (xmp = emp->b_prev)
	    xmp->b_next = nmp;
	else
	    q->q_first = nmp;
	nmp->b_prev = xmp;
	nmp->b_next = emp;
	emp->b_prev = nmp;
    }
    else
    {   /* At end of queue */
	if (xmp = q->q_last)
	{   /* Queue not empty */
	    nmp->b_prev = xmp;
	    nmp->b_next = (mblk_t *)0;
	    xmp->b_next = nmp;
	    q->q_last = nmp;
	}
	else
	{   /* Empty queue */
	    q->q_first = q->q_last = nmp;
	    nmp->b_prev = nmp->b_next = (mblk_t *)0;
	    if ((q->q_flag & (QNOENB|QSERV|QSCHED))==(QSERV|QSCHED))
	    {
		q->q_flag &= ~QSCHED;
		qenable(q);
	    }
	}
    }

    /* Adjust queue size and set FULL if goes over HWM */
    sz = q->q_count;
    while (nmp)
    {
	sz -= (int)(nmp->b_wptr - nmp->b_rptr);
	nmp = nmp->b_cont;
    }
    if ((q->q_count = sz) >= q->q_hiwat)
	q->q_flag |= QFULL;

#ifdef SPLSTR
    splx(s);
#endif
}


/************************/
/*	putbq		*/
/************************/
void 
putbq(queue_t *q, mblk_t *mp)
{
    int type = mp->b_datap->db_type;
    ushort sz;
#ifdef SPLSTR
    int s = splstr();
#endif
    mblk_t *xmp, *omp;

    if (xmp = q->q_first)
    {   /* Queue not empty */
	if (type>QPCTL || xmp->b_datap->db_type<=QPCTL)
	{   /* Put at head (N.B. queue isn't empty) */
	    mp->b_prev = (mblk_t *)0;
	    mp->b_next = xmp;
	    q->q_first = mp;
	    if (type>QPCTL && (q->q_flag & QSERV))
	    {
		qenable(q);
	    }
	}
	else
	{   /* Put before first non-priority message (if any) */
	    while ((omp = xmp, xmp = xmp->b_next) &&
				xmp->b_datap->db_type>QPCTL);
	    mp->b_prev = omp;
	    omp->b_next = mp;
	    if (mp->b_next = xmp)
		xmp->b_prev = mp;
	    else
		q->q_last = mp;
	}
    }
    else
    {   /* Empty queue */
	q->q_first = q->q_last = mp;
	mp->b_prev = mp->b_next = (mblk_t *)0;
	if (type>QPCTL || (q->q_flag & (QNOENB|QSERV|QSCHED))==(QSERV|QSCHED))
	{
	    q->q_flag &= ~QSCHED;
	    qenable(q);
	}
    }

    sz = q->q_count;
    while (mp)
    {
	sz += (int)(mp->b_wptr - mp->b_rptr);
	mp = mp->b_cont;
    }
    if ((q->q_count = sz) >= q->q_hiwat)
	q->q_flag |= QFULL;

#ifdef SPLSTR
    splx(s);
#endif
}


/************************/
/*	putctl		*/
/************************/
int 
putctl(queue_t *q, int type)
{
    register mblk_t *mp;
    mblk_t *allocb();

    if (type!=M_DATA && type!=M_PROTO && type!=M_PCPROTO &&
				       (mp = allocb(1, BPRI_HI)))
    {   /* Got message OK: set up and call 'q's put function */
	mp->b_datap->db_type = (unchar) type;
	mp->b_datap->db_diag = 0x12;
	(*q->q_qinfo->qi_putp)(q, mp);
	return 1;
    }
    return 0;
}


/************************/
/*	putctl1		*/
/************************/
int putctl1(queue_t *q, unchar type, char p)
{
    register mblk_t *mp;
    mblk_t *allocb();
    if (type!=M_DATA && type!=M_PROTO && type!=M_PCPROTO &&
				       (mp = allocb(1, BPRI_HI)))
    {   /* Got message OK: set up and call 'q's put functuon */
	mp->b_datap->db_type = type;
	mp->b_datap->db_diag = 0x13;
	*mp->b_wptr++ = p;
	(*q->q_qinfo->qi_putp)(q, mp);
	return 1;
    }
    return 0;
}


/************************/
/*	putq		*/
/************************/
void 
putq(queue_t *q, mblk_t *mp)
{
    int type = mp->b_datap->db_type;
    ushort sz;
#ifdef SPLSTR
    int s = splstr();
#endif
    mblk_t *xmp, *omp;

    if (xmp = q->q_first)
    {   /* Queue not empty */
	if (type>QPCTL)
	{   /* Priority, so tack onto end of priority msgs on q */

	    if (xmp->b_datap->db_type & QPCTL)
	    {   /* Put before first non-priority message (if any) */
		while ((omp = xmp, xmp = xmp->b_next) &&
					(xmp->b_datap->db_type & QPCTL));
		mp->b_prev = omp;
		omp->b_next = mp;
		if (mp->b_next = xmp)
		    xmp->b_prev = mp;
		else
		    q->q_last = mp;
	    }
	    else
	    {   /* Put at head (N.B. queue isn't empty) */
		mp->b_prev = (mblk_t *)0;
		mp->b_next = xmp;
		q->q_first = mp;
	    }
	    if (q->q_flag & QSERV)
		qenable(q);
	}
	else
	{   /* Not priority, so put at tail (N.B. queue isn't empty) */
	    mp->b_prev = xmp = q->q_last;
	    mp->b_next = (mblk_t *)0;
	    xmp->b_next = mp;
	    q->q_last = mp;
	}
    }
    else
    {   /* Empty queue */
	q->q_first = q->q_last = mp;
	mp->b_prev = mp->b_next = (mblk_t *)0;
	if (type>QPCTL || (q->q_flag & (QNOENB|QSERV|QSCHED))==(QSERV|QSCHED))
	{
	    q->q_flag &= ~QSCHED;
	    qenable(q);
	}
    }

    sz = q->q_count;
    while (mp)
    {
	sz += (int)(mp->b_wptr - mp->b_rptr);
	mp = mp->b_cont;
    }

    if ((q->q_count = sz) >= q->q_hiwat)
	q->q_flag |= QFULL;

#ifdef SPLSTR
    splx(s);
#endif
}


/************************/
/*	qenable		*/
/************************/
int 
qenable(queue_t *q)
{
#ifdef SPLSTR
    int s = splstr();
#endif
    int flag = q->q_flag;

    if (flag & QSERV)
    {
	if ((flag & QENAB)==0)
	{   /* Not yet enabled */
	    q->q_flag |= QENAB;
	    if (runqhead)
		runqtail->q_link = q;
	    else
		runqhead = q;

	    runqtail = q;
	    needsrunq++;
	}
    }
#ifdef SPLSTR
    splx(s);
#endif
    return 1;
}


/************************/
/*	qreply		*/
/************************/
void 
qreply(queue_t *q, mblk_t *mp)
{
    if (q = OTHERQ(q)->q_next)
    {
	(*q->q_qinfo->qi_putp)(q, mp);
	return;
    }
}


/************************/
/*	qsize		*/
/************************/
int 
qsize(queue_t *q)
{
    register mblk_t *mp;
    register sz = 0;
    for (mp = q->q_first; mp; mp = mp->b_next) sz++;
    return sz;
}


/************************/
/*	rmvq		*/
/************************/
void 
rmvq(queue_t *q, mblk_t  *mp)
{
    mblk_t *xmp,*nmp = mp ;
    ushort sz;
#ifdef SPLSTR
    int s = splstr();
#endif
    xmp = mp->b_prev;
    mp = mp->b_next;
    if (xmp)
	xmp->b_next = mp;
    else
	q->q_first = mp;

    if (mp)
	mp->b_prev = xmp;
    else
	q->q_last = xmp;

    nmp->b_prev = nmp->b_next = 0 ;

    sz = q->q_count;
    do
    {
	sz -= (int)(nmp->b_wptr - nmp->b_rptr);
    }
    while (nmp = nmp->b_cont);

     /* High-water mark */
    if ((q->q_count = sz) < q->q_hiwat)
        q->q_flag &= ~QFULL;


    if (q->q_count == 0)
    {
         q->q_flag |= QSCHED;
    }

    if (sz <= q->q_lowat && (q->q_flag & QBACK))
    {
        q->q_flag &= ~QBACK;
        if ((q = q->q_back) && (q->q_flag & QSERV))
            qenable(q);
    }


#ifdef SPLSTR
    splx(s);
#endif
}

/************************/
/*	runq		*/
/************************/
int 
runq()
{
    queue_t *q;

#ifdef SPLSTR
    int s = splstr();
#endif
    if (q = runqhead)
    {
	if (runqhead = q->q_link)
	    q->q_link = (queue_t *)0;
	else
	    runqtail = (queue_t *)0;

	q->q_flag &= ~QENAB;
#ifdef SPLSTR
	splx(s);
#endif
	cnt_arry[QUEUE_Q] = (int3264) q ; 
	cnt_arry[QUEUE_PROC] = (int3264) q->q_qinfo->qi_srvp ;
	cnt_arry[QUEUE_SEMA]++ ;
        /* printf("running q = %x\n",q); */
	(*q->q_qinfo->qi_srvp)(q);
	cnt_arry[QUEUE_SEMA]-- ; 
	return 1;
    }
    return 0;
}


/************************/
/*	unrunq		*/
/************************/
int 
unrunq(queue_t *q)
{
	queue_t *tmpq;
#ifdef SPLSTR
    int s = splstr();
#endif
	if (!q || !(q->q_flag & QENAB)) {
#ifdef SPLSTR
		splx(s);
#endif
		return 0;
	}

	if (q == runqhead) {
		if (!(runqhead = runqhead->q_link))
			runqtail = (queue_t *) 0;
	}
	else {
		for(tmpq = runqhead; tmpq->q_link; tmpq = tmpq->q_link) {
			if (tmpq->q_link == q) {
				if(!(tmpq->q_link = tmpq->q_link->q_link))
					runqtail = tmpq;
				break;
			}
		}
	}

	q->q_flag &= ~QENAB;
#ifdef SPLSTR
	splx(s);
#endif
    return 1;
}

