[apple/xnu.git] / bsd / net / classq / classq.c

/*
 * Copyright (c) 2007-2016 Apple Inc. All rights reserved.
 *
 * @APPLE_OSREFERENCE_LICENSE_HEADER_START@
 *
 * This file contains Original Code and/or Modifications of Original Code
 * as defined in and that are subject to the Apple Public Source License
 * Version 2.0 (the 'License'). You may not use this file except in
 * compliance with the License. The rights granted to you under the License
 * may not be used to create, or enable the creation or redistribution of,
 * unlawful or unlicensed copies of an Apple operating system, or to
 * circumvent, violate, or enable the circumvention or violation of, any
 * terms of an Apple operating system software license agreement.
 *
 * Please obtain a copy of the License at
 * http://www.opensource.apple.com/apsl/ and read it before using this file.
 *
 * The Original Code and all software distributed under the License are
 * distributed on an 'AS IS' basis, WITHOUT WARRANTY OF ANY KIND, EITHER
 * EXPRESS OR IMPLIED, AND APPLE HEREBY DISCLAIMS ALL SUCH WARRANTIES,
 * INCLUDING WITHOUT LIMITATION, ANY WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE, QUIET ENJOYMENT OR NON-INFRINGEMENT.
 * Please see the License for the specific language governing rights and
 * limitations under the License.
 *
 * @APPLE_OSREFERENCE_LICENSE_HEADER_END@
 */

/*
 * Copyright (c) 1991-1997 Regents of the University of California.
 * 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 the Network Research
 *      Group at Lawrence Berkeley Laboratory.
 * 4. Neither the name of the University nor of the Laboratory may be used
 *    to endorse or promote products derived from this software without
 *    specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``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 REGENTS OR CONTRIBUTORS 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/cdefs.h>
#include <sys/param.h>
#include <sys/mbuf.h>
#include <sys/errno.h>
#include <sys/random.h>
#include <sys/kernel_types.h>
#include <sys/sysctl.h>

#include <net/if.h>
#include <net/net_osdep.h>
#include <net/classq/classq.h>

#include <libkern/libkern.h>

u_int32_t classq_verbose = 0;	/* more noise if greater than 1 */

SYSCTL_NODE(_net, OID_AUTO, classq, CTLFLAG_RW|CTLFLAG_LOCKED, 0, "classq");

SYSCTL_UINT(_net_classq, OID_AUTO, verbose, CTLFLAG_RW|CTLFLAG_LOCKED,
	&classq_verbose, 0, "Class queue verbosity level");

void
_qinit(class_queue_t *q, int type, int lim)
{
	MBUFQ_INIT(&q->mbufq);
	qlimit(q) = lim;
	qlen(q) = 0;
	qsize(q) = 0;
	qtype(q) = type;
	qstate(q) = QS_RUNNING;
}

/* add a packet at the tail of the queue */
void
_addq(class_queue_t *q, struct mbuf *m)
{
	MBUFQ_ENQUEUE(&q->mbufq, m);
	qlen(q)++;
	VERIFY(qlen(q) != 0);
	qsize(q) += m_length(m);
}

/* add one or more packets at the tail of the queue */
void
_addq_multi(class_queue_t *q, struct mbuf *m_head, struct mbuf *m_tail,
    u_int32_t cnt, u_int32_t size)
{
	MBUFQ_ENQUEUE_MULTI(&q->mbufq, m_head, m_tail);
	qlen(q) += cnt;
	qsize(q) += size;
}

/* get a packet at the head of the queue */
struct mbuf *
_getq(class_queue_t *q)
{
	struct mbuf *m;

	MBUFQ_DEQUEUE(&q->mbufq, m);
	if (m == NULL) {
		VERIFY(qlen(q) == 0);
		if (qsize(q) > 0)
			qsize(q) = 0;
		return (NULL);
	}
	VERIFY(qlen(q) > 0);
	qlen(q)--;

	/* qsize is an approximation, so adjust if necessary */
	if (((int)qsize(q) - m_length(m)) > 0)
		qsize(q) -= m_length(m);
	else if (qsize(q) != 0)
		qsize(q) = 0;

	return (m);
}

static struct mbuf *
_getq_flow_or_scidx(class_queue_t *q, u_int32_t val, boolean_t isflowid)
{
	struct mbuf *m, *m_tmp;

	MBUFQ_FOREACH_SAFE(m, &q->mbufq, m_tmp) {
		if ((isflowid && (val == 0 || ((m->m_flags & M_PKTHDR) &&
		    m->m_pkthdr.pkt_flowid == val))) ||
		    (!isflowid &&
		    MBUF_SCIDX(mbuf_get_service_class(m)) < val)) {
			/* remove it from the class queue */
			MBUFQ_REMOVE(&q->mbufq, m);
			MBUFQ_NEXT(m) = NULL;
			break;
		}
	}

	if (m != NULL) {
		u_int32_t l = m_length(m);

		VERIFY(qlen(q) > 0);
		qlen(q)--;

		/* qsize is an approximation, so adjust if necessary */
		if (((int)qsize(q) - l) > 0)
			qsize(q) -= l;
		else if (qsize(q) != 0)
			qsize(q) = 0;
	}

	return (m);

}

/* get a packet of a specific flow beginning from the head of the queue */
struct mbuf *
_getq_flow(class_queue_t *q, u_int32_t flow)
{
	return (_getq_flow_or_scidx(q, flow, TRUE));
}

/* Get a packet whose MBUF_SCIDX() < scidx from head of queue */
struct mbuf *
_getq_scidx_lt(class_queue_t *q, u_int32_t scidx)
{
	return (_getq_flow_or_scidx(q, scidx, FALSE));
}

/* get all packets starting from the head of the queue */
struct mbuf *
_getq_all(class_queue_t *q, struct mbuf **last, u_int32_t *qlenp,
    u_int64_t *qsizep)
{
	struct mbuf *m;

	m = MBUFQ_FIRST(&q->mbufq);
	if (last != NULL)
		*last = MBUFQ_LAST(&q->mbufq);
	MBUFQ_INIT(&q->mbufq);

	if (qlenp != NULL)
		*qlenp = qlen(q);
	if (qsizep != NULL)
		*qsizep = qsize(q);

	qlen(q) = 0;
	qsize(q) = 0;

	return (m);
}

/* drop a packet at the tail of the queue */
struct mbuf *
_getq_tail(class_queue_t *q)
{
	struct mq_head *head = &q->mbufq;
	struct mbuf *m = MBUFQ_LAST(head);

	if (m != NULL) {
		struct mbuf *n = MBUFQ_FIRST(head);

		while (n != NULL) {
			struct mbuf *next = MBUFQ_NEXT(n);
			if (next == m) {
				MBUFQ_NEXT(n) = NULL;
				break;
			}
			n = next;
		}
		VERIFY(n != NULL ||
		    (qlen(q) == 1 && m == MBUFQ_FIRST(head)));
		VERIFY(qlen(q) > 0);
		--qlen(q);

		/* qsize is an approximation, so adjust if necessary */
		if (((int)qsize(q) - m_length(m)) > 0)
			qsize(q) -= m_length(m);
		else if (qsize(q) != 0)
			qsize(q) = 0;

		if (qempty(q)) {
			VERIFY(m == MBUFQ_FIRST(head));
			MBUFQ_INIT(head);
		} else {
			VERIFY(n != NULL);
			head->mq_last = &MBUFQ_NEXT(n);
		}
	}
	return (m);
}

/* randomly select a packet in the queue */
struct mbuf *
_getq_random(class_queue_t *q)
{
	struct mq_head *head = &q->mbufq;
	struct mbuf *m = NULL;
	unsigned int n;
	u_int32_t rnd;

	n = qlen(q);
	if (n == 0) {
		VERIFY(MBUFQ_EMPTY(head));
		if (qsize(q) > 0)
			qsize(q) = 0;
		return (NULL);
	}

	m = MBUFQ_FIRST(head);
	read_random(&rnd, sizeof (rnd));
	n = (rnd % n) + 1;

	if (n == 1) {
		if ((MBUFQ_FIRST(head) = MBUFQ_NEXT(m)) == NULL)
			(head)->mq_last = &MBUFQ_FIRST(head);
	} else {
		struct mbuf *p = NULL;

		VERIFY(n > 1);
		while (n--) {
			if (MBUFQ_NEXT(m) == NULL)
				break;
			p = m;
			m = MBUFQ_NEXT(m);
		}
		VERIFY(p != NULL && MBUFQ_NEXT(p) == m);

		if ((MBUFQ_NEXT(p) = MBUFQ_NEXT(m)) == NULL)
			(head)->mq_last = &MBUFQ_NEXT(p);
	}

	VERIFY(qlen(q) > 0);
	--qlen(q);

	/* qsize is an approximation, so adjust if necessary */
	if (((int)qsize(q) - m_length(m)) > 0)
		qsize(q) -= m_length(m);
	else if (qsize(q) != 0)
		qsize(q) = 0;

	MBUFQ_NEXT(m) = NULL;

	return (m);
}

/* remove a packet from the queue */
void
_removeq(class_queue_t *q, struct mbuf *m)
{
	struct mq_head *head = &q->mbufq;
	struct mbuf *m0, **mtail;

	m0 = MBUFQ_FIRST(head);
	if (m0 == NULL)
		return;

	if (m0 != m) {
		while (MBUFQ_NEXT(m0) != m) {
			if (m0 == NULL)
				return;
			m0 = MBUFQ_NEXT(m0);
		}
		mtail = &MBUFQ_NEXT(m0);
	} else {
		mtail = &MBUFQ_FIRST(head);
	}

	*mtail = MBUFQ_NEXT(m);
	if (*mtail == NULL)
		head->mq_last = mtail;

	VERIFY(qlen(q) > 0);
	--qlen(q);

	/* qsize is an approximation, so adjust if necessary */
	if (((int)qsize(q) - m_length(m)) > 0)
		qsize(q) -= m_length(m);
	else if (qsize(q) != 0)
		qsize(q) = 0;

	MBUFQ_NEXT(m) = NULL;
}

void
_flushq(class_queue_t *q)
{
	(void) _flushq_flow(q, 0, NULL, NULL);
}

void
_flushq_flow(class_queue_t *q, u_int32_t flow, u_int32_t *cnt, u_int32_t *len)
{
	MBUFQ_HEAD(mq_freeq) freeq;
	struct mbuf *m, *m_tmp;
	u_int32_t c = 0, l = 0;

	MBUFQ_INIT(&freeq);

	MBUFQ_FOREACH_SAFE(m, &q->mbufq, m_tmp) {
		if (flow == 0 || ((m->m_flags & M_PKTHDR) &&
		    m->m_pkthdr.pkt_flowid == flow)) {
			/* remove it from the class queue */
			MBUFQ_REMOVE(&q->mbufq, m);
			MBUFQ_NEXT(m) = NULL;

			/* and add it to the free queue */
			MBUFQ_ENQUEUE(&freeq, m);

			l += m_length(m);
			c++;
		}
	}
	VERIFY(c == 0 || !MBUFQ_EMPTY(&freeq));

	if (c > 0) {
		VERIFY(qlen(q) >= c);
		qlen(q) -= c;

		/* qsize is an approximation, so adjust if necessary */
		if (((int)qsize(q) - l) > 0)
			qsize(q) -= l;
		else if (qsize(q) != 0)
			qsize(q) = 0;
	}

	if (!MBUFQ_EMPTY(&freeq))
		m_freem_list(MBUFQ_FIRST(&freeq));

	if (cnt != NULL)
		*cnt = c;
	if (len != NULL)
		*len = l;
}
Commit	Line	Data
316670eb	1	/*
39037602	2	* Copyright (c) 2007-2016 Apple Inc. All rights reserved.
316670eb A	3	*
	4	* @APPLE_OSREFERENCE_LICENSE_HEADER_START@
	5	*
	6	* This file contains Original Code and/or Modifications of Original Code
	7	* as defined in and that are subject to the Apple Public Source License
	8	* Version 2.0 (the 'License'). You may not use this file except in
	9	* compliance with the License. The rights granted to you under the License
	10	* may not be used to create, or enable the creation or redistribution of,
	11	* unlawful or unlicensed copies of an Apple operating system, or to
	12	* circumvent, violate, or enable the circumvention or violation of, any
	13	* terms of an Apple operating system software license agreement.
	14	*
	15	* Please obtain a copy of the License at
	16	* http://www.opensource.apple.com/apsl/ and read it before using this file.
	17	*
	18	* The Original Code and all software distributed under the License are
	19	* distributed on an 'AS IS' basis, WITHOUT WARRANTY OF ANY KIND, EITHER
	20	* EXPRESS OR IMPLIED, AND APPLE HEREBY DISCLAIMS ALL SUCH WARRANTIES,
	21	* INCLUDING WITHOUT LIMITATION, ANY WARRANTIES OF MERCHANTABILITY,
	22	* FITNESS FOR A PARTICULAR PURPOSE, QUIET ENJOYMENT OR NON-INFRINGEMENT.
	23	* Please see the License for the specific language governing rights and
	24	* limitations under the License.
	25	*
	26	* @APPLE_OSREFERENCE_LICENSE_HEADER_END@
	27	*/
	28
	29	/*
	30	* Copyright (c) 1991-1997 Regents of the University of California.
	31	* All rights reserved.
	32	*
	33	* Redistribution and use in source and binary forms, with or without
	34	* modification, are permitted provided that the following conditions
	35	* are met:
	36	* 1. Redistributions of source code must retain the above copyright
	37	* notice, this list of conditions and the following disclaimer.
	38	* 2. Redistributions in binary form must reproduce the above copyright
	39	* notice, this list of conditions and the following disclaimer in the
	40	* documentation and/or other materials provided with the distribution.
	41	* 3. All advertising materials mentioning features or use of this software
	42	* must display the following acknowledgement:
	43	* This product includes software developed by the Network Research
	44	* Group at Lawrence Berkeley Laboratory.
	45	* 4. Neither the name of the University nor of the Laboratory may be used
	46	* to endorse or promote products derived from this software without
	47	* specific prior written permission.
	48	*
	49	* THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND
	50	* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
	51	* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
	52	* ARE DISCLAIMED. IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE
	53	* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
	54	* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
	55	* OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
	56	* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
	57	* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
	58	* OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
	59	* SUCH DAMAGE.
	60	*/
	61
	62	#include <sys/cdefs.h>
	63	#include <sys/param.h>
	64	#include <sys/mbuf.h>
	65	#include <sys/errno.h>
	66	#include <sys/random.h>
67	#include <sys/kernel_types.h>
68	#include <sys/sysctl.h>
69
70	#include <net/if.h>
71	#include <net/net_osdep.h>
72	#include <net/classq/classq.h>
73
74	#include <libkern/libkern.h>
75
3e170ce0	76	u_int32_t classq_verbose = 0; /* more noise if greater than 1 */
316670eb A	77
	78	SYSCTL_NODE(_net, OID_AUTO, classq, CTLFLAG_RW\|CTLFLAG_LOCKED, 0, "classq");
	79
	80	SYSCTL_UINT(_net_classq, OID_AUTO, verbose, CTLFLAG_RW\|CTLFLAG_LOCKED,
	81	&classq_verbose, 0, "Class queue verbosity level");
	82
	83	void
	84	_qinit(class_queue_t *q, int type, int lim)
	85	{
	86	MBUFQ_INIT(&q->mbufq);
	87	qlimit(q) = lim;
	88	qlen(q) = 0;
	89	qsize(q) = 0;
	90	qtype(q) = type;
	91	qstate(q) = QS_RUNNING;
	92	}
	93
	94	/* add a packet at the tail of the queue */
	95	void
	96	_addq(class_queue_t q, struct mbuf m)
	97	{
	98	MBUFQ_ENQUEUE(&q->mbufq, m);
	99	qlen(q)++;
	100	VERIFY(qlen(q) != 0);
	101	qsize(q) += m_length(m);
	102	}
	103
	104	/* add one or more packets at the tail of the queue */
	105	void
	106	_addq_multi(class_queue_t q, struct mbuf m_head, struct mbuf *m_tail,
	107	u_int32_t cnt, u_int32_t size)
	108	{
	109	MBUFQ_ENQUEUE_MULTI(&q->mbufq, m_head, m_tail);
	110	qlen(q) += cnt;
	111	qsize(q) += size;
	112	}
	113
	114	/* get a packet at the head of the queue */
	115	struct mbuf *
	116	_getq(class_queue_t *q)
	117	{
	118	struct mbuf *m;
	119
	120	MBUFQ_DEQUEUE(&q->mbufq, m);
	121	if (m == NULL) {
	122	VERIFY(qlen(q) == 0);
	123	if (qsize(q) > 0)
	124	qsize(q) = 0;
	125	return (NULL);
	126	}
	127	VERIFY(qlen(q) > 0);
	128	qlen(q)--;
	129
	130	/* qsize is an approximation, so adjust if necessary */
	131	if (((int)qsize(q) - m_length(m)) > 0)
	132	qsize(q) -= m_length(m);
	133	else if (qsize(q) != 0)
	134	qsize(q) = 0;
	135
	136	return (m);
	137	}
	138
39037602 A	139	static struct mbuf *
39037602 A	140	_getq_flow_or_scidx(class_queue_t *q, u_int32_t val, boolean_t isflowid)
316670eb A	141	{
	142	struct mbuf m, m_tmp;
	143
	144	MBUFQ_FOREACH_SAFE(m, &q->mbufq, m_tmp) {
39037602 A	145	if ((isflowid && (val == 0 \|\| ((m->m_flags & M_PKTHDR) &&
	146	m->m_pkthdr.pkt_flowid == val))) \|\|
	147	(!isflowid &&
	148	MBUF_SCIDX(mbuf_get_service_class(m)) < val)) {
316670eb A	149	/* remove it from the class queue */
	150	MBUFQ_REMOVE(&q->mbufq, m);
	151	MBUFQ_NEXT(m) = NULL;
	152	break;
	153	}
	154	}
	155
	156	if (m != NULL) {
	157	u_int32_t l = m_length(m);
	158
	159	VERIFY(qlen(q) > 0);
	160	qlen(q)--;
	161
	162	/* qsize is an approximation, so adjust if necessary */
	163	if (((int)qsize(q) - l) > 0)
	164	qsize(q) -= l;
	165	else if (qsize(q) != 0)
	166	qsize(q) = 0;
	167	}
	168
	169	return (m);
39037602 A	170
	171	}
	172
	173	/* get a packet of a specific flow beginning from the head of the queue */
	174	struct mbuf *
	175	_getq_flow(class_queue_t *q, u_int32_t flow)
	176	{
	177	return (_getq_flow_or_scidx(q, flow, TRUE));
	178	}
	179
	180	/* Get a packet whose MBUF_SCIDX() < scidx from head of queue */
	181	struct mbuf *
	182	_getq_scidx_lt(class_queue_t *q, u_int32_t scidx)
	183	{
	184	return (_getq_flow_or_scidx(q, scidx, FALSE));
316670eb A	185	}
	186
	187	/* get all packets starting from the head of the queue */
	188	struct mbuf *
39037602 A	189	_getq_all(class_queue_t q, struct mbuf last, u_int32_t qlenp,
39037602 A	190	u_int64_t *qsizep)
316670eb A	191	{
	192	struct mbuf *m;
	193
	194	m = MBUFQ_FIRST(&q->mbufq);
39037602 A	195	if (last != NULL)
39037602 A	196	*last = MBUFQ_LAST(&q->mbufq);
316670eb	197	MBUFQ_INIT(&q->mbufq);
39037602 A	198
	199	if (qlenp != NULL)
	200	*qlenp = qlen(q);
	201	if (qsizep != NULL)
	202	*qsizep = qsize(q);
	203
316670eb A	204	qlen(q) = 0;
	205	qsize(q) = 0;
	206
	207	return (m);
	208	}
	209
	210	/* drop a packet at the tail of the queue */
	211	struct mbuf *
	212	_getq_tail(class_queue_t *q)
	213	{
	214	struct mq_head *head = &q->mbufq;
	215	struct mbuf *m = MBUFQ_LAST(head);
	216
	217	if (m != NULL) {
	218	struct mbuf *n = MBUFQ_FIRST(head);
	219
	220	while (n != NULL) {
	221	struct mbuf *next = MBUFQ_NEXT(n);
	222	if (next == m) {
	223	MBUFQ_NEXT(n) = NULL;
	224	break;
	225	}
	226	n = next;
	227	}
	228	VERIFY(n != NULL \|\|
	229	(qlen(q) == 1 && m == MBUFQ_FIRST(head)));
	230	VERIFY(qlen(q) > 0);
	231	--qlen(q);
	232
	233	/* qsize is an approximation, so adjust if necessary */
	234	if (((int)qsize(q) - m_length(m)) > 0)
	235	qsize(q) -= m_length(m);
	236	else if (qsize(q) != 0)
	237	qsize(q) = 0;
	238
	239	if (qempty(q)) {
39037602	240	VERIFY(m == MBUFQ_FIRST(head));
316670eb A	241	MBUFQ_INIT(head);
	242	} else {
	243	VERIFY(n != NULL);
	244	head->mq_last = &MBUFQ_NEXT(n);
	245	}
	246	}
	247	return (m);
	248	}
	249
	250	/* randomly select a packet in the queue */
	251	struct mbuf *
	252	_getq_random(class_queue_t *q)
	253	{
	254	struct mq_head *head = &q->mbufq;
	255	struct mbuf *m = NULL;
	256	unsigned int n;
	257	u_int32_t rnd;
	258
	259	n = qlen(q);
	260	if (n == 0) {
	261	VERIFY(MBUFQ_EMPTY(head));
	262	if (qsize(q) > 0)
	263	qsize(q) = 0;
	264	return (NULL);
	265	}
	266
	267	m = MBUFQ_FIRST(head);
	268	read_random(&rnd, sizeof (rnd));
	269	n = (rnd % n) + 1;
	270
	271	if (n == 1) {
	272	if ((MBUFQ_FIRST(head) = MBUFQ_NEXT(m)) == NULL)
	273	(head)->mq_last = &MBUFQ_FIRST(head);
	274	} else {
	275	struct mbuf *p = NULL;
	276
	277	VERIFY(n > 1);
	278	while (n--) {
	279	if (MBUFQ_NEXT(m) == NULL)
	280	break;
	281	p = m;
	282	m = MBUFQ_NEXT(m);
	283	}
	284	VERIFY(p != NULL && MBUFQ_NEXT(p) == m);
	285
	286	if ((MBUFQ_NEXT(p) = MBUFQ_NEXT(m)) == NULL)
	287	(head)->mq_last = &MBUFQ_NEXT(p);
	288	}
	289
	290	VERIFY(qlen(q) > 0);
	291	--qlen(q);
	292
	293	/* qsize is an approximation, so adjust if necessary */
	294	if (((int)qsize(q) - m_length(m)) > 0)
	295	qsize(q) -= m_length(m);
	296	else if (qsize(q) != 0)
	297	qsize(q) = 0;
	298
	299	MBUFQ_NEXT(m) = NULL;
	300
	301	return (m);
	302	}
	303
	304	/* remove a packet from the queue */
305	void
306	_removeq(class_queue_t q, struct mbuf m)
307	{
308	struct mq_head *head = &q->mbufq;
309	struct mbuf m0, *mtail;
310
311	m0 = MBUFQ_FIRST(head);
312	if (m0 == NULL)
313	return;
314
315	if (m0 != m) {
316	while (MBUFQ_NEXT(m0) != m) {
317	if (m0 == NULL)
318	return;
319	m0 = MBUFQ_NEXT(m0);
320	}
321	mtail = &MBUFQ_NEXT(m0);
322	} else {
323	mtail = &MBUFQ_FIRST(head);
324	}
325
326	*mtail = MBUFQ_NEXT(m);
327	if (*mtail == NULL)
328	head->mq_last = mtail;
329
330	VERIFY(qlen(q) > 0);
331	--qlen(q);
332
333	/* qsize is an approximation, so adjust if necessary */
334	if (((int)qsize(q) - m_length(m)) > 0)
335	qsize(q) -= m_length(m);
336	else if (qsize(q) != 0)
337	qsize(q) = 0;
338
339	MBUFQ_NEXT(m) = NULL;
340	}
341
342	void
343	_flushq(class_queue_t *q)
344	{
345	(void) _flushq_flow(q, 0, NULL, NULL);
346	}
347
348	void
349	_flushq_flow(class_queue_t q, u_int32_t flow, u_int32_t cnt, u_int32_t *len)
350	{
351	MBUFQ_HEAD(mq_freeq) freeq;
352	struct mbuf m, m_tmp;
353	u_int32_t c = 0, l = 0;
354
355	MBUFQ_INIT(&freeq);
356
357	MBUFQ_FOREACH_SAFE(m, &q->mbufq, m_tmp) {
358	if (flow == 0 \|\| ((m->m_flags & M_PKTHDR) &&
39236c6e	359	m->m_pkthdr.pkt_flowid == flow)) {
316670eb A	360	/* remove it from the class queue */
	361	MBUFQ_REMOVE(&q->mbufq, m);
	362	MBUFQ_NEXT(m) = NULL;
	363
	364	/* and add it to the free queue */
	365	MBUFQ_ENQUEUE(&freeq, m);
	366
	367	l += m_length(m);
	368	c++;
	369	}
	370	}
	371	VERIFY(c == 0 \|\| !MBUFQ_EMPTY(&freeq));
	372
	373	if (c > 0) {
	374	VERIFY(qlen(q) >= c);
	375	qlen(q) -= c;
	376
	377	/* qsize is an approximation, so adjust if necessary */
	378	if (((int)qsize(q) - l) > 0)
	379	qsize(q) -= l;
	380	else if (qsize(q) != 0)
	381	qsize(q) = 0;
	382	}
	383
	384	if (!MBUFQ_EMPTY(&freeq))
	385	m_freem_list(MBUFQ_FIRST(&freeq));
	386
	387	if (cnt != NULL)
	388	*cnt = c;
	389	if (len != NULL)
	390	*len = l;
	391	}