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

/*
 * Copyright (c) 2007-2017 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, classq_pkt_type_t ptype)
{
	switch (ptype) {
	case QP_MBUF:
		MBUFQ_INIT(&qmbufq(q));
		break;


	default:
		VERIFY(0);
		/* NOTREACHED */
	}

	qlimit(q) = lim;
	qlen(q) = 0;
	qsize(q) = 0;
	qtype(q) = type;
	qptype(q) = ptype;
	qstate(q) = QS_RUNNING;
}

/* add a packet at the tail of the queue */
void
_addq(class_queue_t *q, void *pkt)
{
	uint32_t size = 0;

	switch (qptype(q)) {
	case QP_MBUF: {
		struct mbuf *m = pkt;
		MBUFQ_ENQUEUE(&qmbufq(q), m);
		size = m_length(m);
		break;
	}


	default:
		VERIFY(0);
		/* NOTREACHED */
	}

	qlen(q)++;
	VERIFY(qlen(q) != 0);
	qsize(q) += size;
}

/* add one or more packets at the tail of the queue */
void
_addq_multi(class_queue_t *q, void *pkt_head, void *pkt_tail,
    u_int32_t cnt, u_int32_t size)
{
	switch (qptype(q)) {
	case QP_MBUF: {
		struct mbuf *m_head = pkt_head;
		struct mbuf *m_tail = pkt_tail;
		MBUFQ_ENQUEUE_MULTI(&qmbufq(q), m_head, m_tail);
		break;
	}


	default:
		VERIFY(0);
		/* NOTREACHED */
	}

	qlen(q) += cnt;
	qsize(q) += size;
}

/* get a packet at the head of the queue */
void *
_getq(class_queue_t *q)
{
	void *pkt = NULL;
	uint32_t pkt_len;

	switch (qptype(q)) {
	case QP_MBUF: {
		struct mbuf *m;
		MBUFQ_DEQUEUE(&qmbufq(q), m);
		if (m != NULL) {
			pkt_len = m_length(m);
			pkt = m;
		}
		break;
	}


	default:
		VERIFY(0);
		/* NOTREACHED */
	}

	if (pkt == 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) - pkt_len) > 0)
		qsize(q) -= pkt_len;
	else if (qsize(q) != 0)
		qsize(q) = 0;

	return (pkt);
}

static void *
_getq_flow_or_scidx(class_queue_t *q, u_int32_t val, boolean_t isflowid)
{
	void *pkt = NULL;
	uint32_t pkt_len;

	switch (qptype(q)) {
	case QP_MBUF: {
		struct mbuf *m, *m_tmp;

		MBUFQ_FOREACH_SAFE(m, &qmbufq(q), 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(&qmbufq(q), m);
				MBUFQ_NEXT(m) = NULL;
				break;
			}
		}
		if (m != NULL) {
			pkt = m;
			pkt_len = m_length(m);
		}
		break;
	}


	default:
		VERIFY(0);
		/* NOTREACHED */
	}

	if (pkt != NULL) {
		VERIFY(qlen(q) > 0);
		qlen(q)--;

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

	return (pkt);
}

/* get a packet of a specific flow beginning from the head of the queue */
void *
_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 */
void *
_getq_scidx_lt(class_queue_t *q, u_int32_t scidx)
{
	return (_getq_flow_or_scidx(q, scidx, FALSE));
}

/* get all packets (chained) starting from the head of the queue */
void *
_getq_all(class_queue_t *q, void **last, u_int32_t *qlenp,
    u_int64_t *qsizep)
{
	void *pkt = NULL;

	switch (qptype(q)) {
	case QP_MBUF:
		pkt = MBUFQ_FIRST(&qmbufq(q));
		if (last != NULL)
			*last = MBUFQ_LAST(&qmbufq(q));
		MBUFQ_INIT(&qmbufq(q));
		break;


	default:
		VERIFY(0);
		/* NOTREACHED */
	}

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

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

	return (pkt);
}

static inline struct mbuf *
_getq_tail_mbuf(class_queue_t *q)
{
	struct mq_head *head = &qmbufq(q);
	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);
}

/* drop a packet at the tail of the queue */
void *
_getq_tail(class_queue_t *q)
{
	void *t = NULL;

	switch (qptype(q)) {
	case QP_MBUF:
		t = _getq_tail_mbuf(q);
		break;

	default:
		VERIFY(0);
		/* NOTREACHED */
	}

	return (t);
}

static inline struct mbuf *
_getq_random_mbuf(class_queue_t *q)
{
	struct mq_head *head = &qmbufq(q);
	struct mbuf *m = NULL;
	unsigned int n;
	u_int32_t rnd;

	/* XXX: Add support for Kernel packet when needed */
	VERIFY((qptype(q) == QP_MBUF));

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

	m = MBUFQ_FIRST(head);
	read_frandom(&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);
}

/* randomly select a packet in the queue */
void *
_getq_random(class_queue_t *q)
{
	void *r = NULL;

	switch (qptype(q)) {
	case QP_MBUF:
		r = _getq_random_mbuf(q);
		break;

	default:
		VERIFY(0);
		/* NOTREACHED */
	}

	return (r);
}

static inline void
_removeq_mbuf(class_queue_t *q, struct mbuf *m)
{
	struct mq_head *head = &qmbufq(q);
	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;
}

/* remove a packet from the queue */
void
_removeq(class_queue_t *q, void *pkt)
{
	switch (qptype(q)) {
	case QP_MBUF:
		_removeq_mbuf(q, pkt);
		break;

	default:
		VERIFY(0);
		/* NOTREACHED */
	}
}

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

static inline void
_flushq_flow_mbuf(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, &qmbufq(q), m_tmp) {
		if (flow == 0 || ((m->m_flags & M_PKTHDR) &&
		    m->m_pkthdr.pkt_flowid == flow)) {
			/* remove it from the class queue */
			MBUFQ_REMOVE(&qmbufq(q), 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;
}

void
_flushq_flow(class_queue_t *q, u_int32_t flow, u_int32_t *cnt, u_int32_t *len)
{
	switch (qptype(q)) {
	case QP_MBUF:
		_flushq_flow_mbuf(q, flow, cnt, len);
		break;

	default:
		VERIFY(0);
		/* NOTREACHED */
	}
}
Commit	Line	Data
316670eb	1	/*
5ba3f43e	2	* Copyright (c) 2007-2017 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
5ba3f43e	76
3e170ce0	77	u_int32_t classq_verbose = 0; /* more noise if greater than 1 */
316670eb A	78
	79	SYSCTL_NODE(_net, OID_AUTO, classq, CTLFLAG_RW\|CTLFLAG_LOCKED, 0, "classq");
	80
	81	SYSCTL_UINT(_net_classq, OID_AUTO, verbose, CTLFLAG_RW\|CTLFLAG_LOCKED,
	82	&classq_verbose, 0, "Class queue verbosity level");
	83
	84	void
5ba3f43e	85	_qinit(class_queue_t *q, int type, int lim, classq_pkt_type_t ptype)
316670eb	86	{
5ba3f43e A	87	switch (ptype) {
	88	case QP_MBUF:
	89	MBUFQ_INIT(&qmbufq(q));
	90	break;
	91
	92
	93	default:
	94	VERIFY(0);
	95	/* NOTREACHED */
	96	}
	97
316670eb A	98	qlimit(q) = lim;
	99	qlen(q) = 0;
	100	qsize(q) = 0;
	101	qtype(q) = type;
5ba3f43e	102	qptype(q) = ptype;
316670eb A	103	qstate(q) = QS_RUNNING;
	104	}
	105
	106	/* add a packet at the tail of the queue */
	107	void
5ba3f43e	108	_addq(class_queue_t q, void pkt)
316670eb	109	{
5ba3f43e A	110	uint32_t size = 0;
	111
	112	switch (qptype(q)) {
	113	case QP_MBUF: {
	114	struct mbuf *m = pkt;
	115	MBUFQ_ENQUEUE(&qmbufq(q), m);
	116	size = m_length(m);
	117	break;
	118	}
	119
	120
	121	default:
	122	VERIFY(0);
	123	/* NOTREACHED */
	124	}
	125
316670eb A	126	qlen(q)++;
316670eb A	127	VERIFY(qlen(q) != 0);
5ba3f43e	128	qsize(q) += size;
316670eb A	129	}
	130
	131	/* add one or more packets at the tail of the queue */
	132	void
5ba3f43e	133	_addq_multi(class_queue_t q, void pkt_head, void *pkt_tail,
316670eb A	134	u_int32_t cnt, u_int32_t size)
316670eb A	135	{
5ba3f43e A	136	switch (qptype(q)) {
	137	case QP_MBUF: {
	138	struct mbuf *m_head = pkt_head;
	139	struct mbuf *m_tail = pkt_tail;
	140	MBUFQ_ENQUEUE_MULTI(&qmbufq(q), m_head, m_tail);
	141	break;
	142	}
	143
	144
	145	default:
	146	VERIFY(0);
	147	/* NOTREACHED */
	148	}
	149
316670eb A	150	qlen(q) += cnt;
	151	qsize(q) += size;
	152	}
	153
	154	/* get a packet at the head of the queue */
5ba3f43e	155	void *
316670eb A	156	_getq(class_queue_t *q)
316670eb A	157	{
5ba3f43e A	158	void *pkt = NULL;
	159	uint32_t pkt_len;
	160
	161	switch (qptype(q)) {
	162	case QP_MBUF: {
	163	struct mbuf *m;
	164	MBUFQ_DEQUEUE(&qmbufq(q), m);
	165	if (m != NULL) {
	166	pkt_len = m_length(m);
	167	pkt = m;
	168	}
	169	break;
	170	}
	171
	172
	173	default:
	174	VERIFY(0);
	175	/* NOTREACHED */
	176	}
316670eb	177
5ba3f43e	178	if (pkt == NULL) {
316670eb A	179	VERIFY(qlen(q) == 0);
	180	if (qsize(q) > 0)
	181	qsize(q) = 0;
	182	return (NULL);
	183	}
	184	VERIFY(qlen(q) > 0);
	185	qlen(q)--;
	186
	187	/* qsize is an approximation, so adjust if necessary */
5ba3f43e A	188	if (((int)qsize(q) - pkt_len) > 0)
5ba3f43e A	189	qsize(q) -= pkt_len;
316670eb A	190	else if (qsize(q) != 0)
	191	qsize(q) = 0;
	192
5ba3f43e	193	return (pkt);
316670eb A	194	}
316670eb A	195
5ba3f43e	196	static void *
39037602	197	_getq_flow_or_scidx(class_queue_t *q, u_int32_t val, boolean_t isflowid)
316670eb	198	{
5ba3f43e A	199	void *pkt = NULL;
	200	uint32_t pkt_len;
	201
	202	switch (qptype(q)) {
	203	case QP_MBUF: {
	204	struct mbuf m, m_tmp;
	205
	206	MBUFQ_FOREACH_SAFE(m, &qmbufq(q), m_tmp) {
	207	if ((isflowid && (val == 0 \|\|
	208	((m->m_flags & M_PKTHDR) &&
	209	m->m_pkthdr.pkt_flowid == val))) \|\|
	210	(!isflowid &&
	211	MBUF_SCIDX(mbuf_get_service_class(m)) < val)) {
	212	/* remove it from the class queue */
	213	MBUFQ_REMOVE(&qmbufq(q), m);
	214	MBUFQ_NEXT(m) = NULL;
	215	break;
	216	}
	217	}
	218	if (m != NULL) {
	219	pkt = m;
	220	pkt_len = m_length(m);
316670eb	221	}
5ba3f43e	222	break;
316670eb A	223	}
316670eb A	224
316670eb	225
5ba3f43e A	226	default:
	227	VERIFY(0);
	228	/* NOTREACHED */
	229	}
	230
	231	if (pkt != NULL) {
316670eb A	232	VERIFY(qlen(q) > 0);
	233	qlen(q)--;
	234
	235	/* qsize is an approximation, so adjust if necessary */
5ba3f43e A	236	if (((int)qsize(q) - pkt_len) > 0)
5ba3f43e A	237	qsize(q) -= pkt_len;
316670eb A	238	else if (qsize(q) != 0)
	239	qsize(q) = 0;
	240	}
	241
5ba3f43e	242	return (pkt);
39037602 A	243	}
	244
	245	/* get a packet of a specific flow beginning from the head of the queue */
5ba3f43e	246	void *
39037602 A	247	_getq_flow(class_queue_t *q, u_int32_t flow)
	248	{
	249	return (_getq_flow_or_scidx(q, flow, TRUE));
	250	}
	251
	252	/* Get a packet whose MBUF_SCIDX() < scidx from head of queue */
5ba3f43e	253	void *
39037602 A	254	_getq_scidx_lt(class_queue_t *q, u_int32_t scidx)
	255	{
	256	return (_getq_flow_or_scidx(q, scidx, FALSE));
316670eb A	257	}
316670eb A	258
5ba3f43e A	259	/* get all packets (chained) starting from the head of the queue */
	260	void *
	261	_getq_all(class_queue_t q, void last, u_int32_t qlenp,
39037602	262	u_int64_t *qsizep)
316670eb	263	{
5ba3f43e A	264	void *pkt = NULL;
	265
	266	switch (qptype(q)) {
	267	case QP_MBUF:
	268	pkt = MBUFQ_FIRST(&qmbufq(q));
	269	if (last != NULL)
	270	*last = MBUFQ_LAST(&qmbufq(q));
	271	MBUFQ_INIT(&qmbufq(q));
	272	break;
316670eb	273
5ba3f43e A	274
	275	default:
	276	VERIFY(0);
	277	/* NOTREACHED */
	278	}
39037602 A	279
	280	if (qlenp != NULL)
	281	*qlenp = qlen(q);
	282	if (qsizep != NULL)
	283	*qsizep = qsize(q);
	284
316670eb A	285	qlen(q) = 0;
	286	qsize(q) = 0;
	287
5ba3f43e	288	return (pkt);
316670eb A	289	}
316670eb A	290
5ba3f43e A	291	static inline struct mbuf *
5ba3f43e A	292	_getq_tail_mbuf(class_queue_t *q)
316670eb	293	{
5ba3f43e	294	struct mq_head *head = &qmbufq(q);
316670eb A	295	struct mbuf *m = MBUFQ_LAST(head);
	296
	297	if (m != NULL) {
	298	struct mbuf *n = MBUFQ_FIRST(head);
	299
	300	while (n != NULL) {
	301	struct mbuf *next = MBUFQ_NEXT(n);
	302	if (next == m) {
	303	MBUFQ_NEXT(n) = NULL;
	304	break;
	305	}
	306	n = next;
	307	}
	308	VERIFY(n != NULL \|\|
	309	(qlen(q) == 1 && m == MBUFQ_FIRST(head)));
	310	VERIFY(qlen(q) > 0);
	311	--qlen(q);
	312
	313	/* qsize is an approximation, so adjust if necessary */
	314	if (((int)qsize(q) - m_length(m)) > 0)
	315	qsize(q) -= m_length(m);
	316	else if (qsize(q) != 0)
	317	qsize(q) = 0;
	318
	319	if (qempty(q)) {
39037602	320	VERIFY(m == MBUFQ_FIRST(head));
316670eb A	321	MBUFQ_INIT(head);
	322	} else {
	323	VERIFY(n != NULL);
	324	head->mq_last = &MBUFQ_NEXT(n);
	325	}
	326	}
	327	return (m);
	328	}
	329
5ba3f43e A	330	/* drop a packet at the tail of the queue */
	331	void *
	332	_getq_tail(class_queue_t *q)
	333	{
	334	void *t = NULL;
	335
	336	switch (qptype(q)) {
	337	case QP_MBUF:
	338	t = _getq_tail_mbuf(q);
	339	break;
	340
	341	default:
	342	VERIFY(0);
	343	/* NOTREACHED */
	344	}
	345
	346	return (t);
	347	}
	348
	349	static inline struct mbuf *
	350	_getq_random_mbuf(class_queue_t *q)
316670eb	351	{
5ba3f43e	352	struct mq_head *head = &qmbufq(q);
316670eb A	353	struct mbuf *m = NULL;
	354	unsigned int n;
	355	u_int32_t rnd;
	356
5ba3f43e A	357	/* XXX: Add support for Kernel packet when needed */
	358	VERIFY((qptype(q) == QP_MBUF));
	359
316670eb A	360	n = qlen(q);
	361	if (n == 0) {
	362	VERIFY(MBUFQ_EMPTY(head));
	363	if (qsize(q) > 0)
	364	qsize(q) = 0;
	365	return (NULL);
	366	}
	367
	368	m = MBUFQ_FIRST(head);
5ba3f43e	369	read_frandom(&rnd, sizeof (rnd));
316670eb A	370	n = (rnd % n) + 1;
	371
	372	if (n == 1) {
	373	if ((MBUFQ_FIRST(head) = MBUFQ_NEXT(m)) == NULL)
	374	(head)->mq_last = &MBUFQ_FIRST(head);
	375	} else {
	376	struct mbuf *p = NULL;
	377
	378	VERIFY(n > 1);
	379	while (n--) {
	380	if (MBUFQ_NEXT(m) == NULL)
	381	break;
	382	p = m;
	383	m = MBUFQ_NEXT(m);
	384	}
	385	VERIFY(p != NULL && MBUFQ_NEXT(p) == m);
	386
	387	if ((MBUFQ_NEXT(p) = MBUFQ_NEXT(m)) == NULL)
	388	(head)->mq_last = &MBUFQ_NEXT(p);
	389	}
	390
	391	VERIFY(qlen(q) > 0);
	392	--qlen(q);
	393
	394	/* qsize is an approximation, so adjust if necessary */
	395	if (((int)qsize(q) - m_length(m)) > 0)
	396	qsize(q) -= m_length(m);
	397	else if (qsize(q) != 0)
	398	qsize(q) = 0;
	399
	400	MBUFQ_NEXT(m) = NULL;
	401
	402	return (m);
	403	}
	404
5ba3f43e A	405	/* randomly select a packet in the queue */
	406	void *
	407	_getq_random(class_queue_t *q)
	408	{
	409	void *r = NULL;
	410
	411	switch (qptype(q)) {
	412	case QP_MBUF:
	413	r = _getq_random_mbuf(q);
	414	break;
	415
	416	default:
	417	VERIFY(0);
	418	/* NOTREACHED */
	419	}
	420
	421	return (r);
	422	}
	423
	424	static inline void
	425	_removeq_mbuf(class_queue_t q, struct mbuf m)
316670eb	426	{
5ba3f43e	427	struct mq_head *head = &qmbufq(q);
316670eb A	428	struct mbuf m0, *mtail;
	429
	430	m0 = MBUFQ_FIRST(head);
	431	if (m0 == NULL)
	432	return;
	433
	434	if (m0 != m) {
	435	while (MBUFQ_NEXT(m0) != m) {
	436	if (m0 == NULL)
	437	return;
	438	m0 = MBUFQ_NEXT(m0);
	439	}
	440	mtail = &MBUFQ_NEXT(m0);
	441	} else {
	442	mtail = &MBUFQ_FIRST(head);
	443	}
	444
	445	*mtail = MBUFQ_NEXT(m);
	446	if (*mtail == NULL)
	447	head->mq_last = mtail;
	448
	449	VERIFY(qlen(q) > 0);
	450	--qlen(q);
	451
	452	/* qsize is an approximation, so adjust if necessary */
	453	if (((int)qsize(q) - m_length(m)) > 0)
	454	qsize(q) -= m_length(m);
	455	else if (qsize(q) != 0)
	456	qsize(q) = 0;
	457
	458	MBUFQ_NEXT(m) = NULL;
	459	}
	460
5ba3f43e A	461	/* remove a packet from the queue */
	462	void
	463	_removeq(class_queue_t q, void pkt)
	464	{
	465	switch (qptype(q)) {
	466	case QP_MBUF:
	467	_removeq_mbuf(q, pkt);
	468	break;
	469
	470	default:
	471	VERIFY(0);
	472	/* NOTREACHED */
	473	}
	474	}
	475
316670eb A	476	void
	477	_flushq(class_queue_t *q)
	478	{
	479	(void) _flushq_flow(q, 0, NULL, NULL);
	480	}
	481
5ba3f43e A	482	static inline void
	483	_flushq_flow_mbuf(class_queue_t q, u_int32_t flow, u_int32_t cnt,
	484	u_int32_t *len)
316670eb A	485	{
	486	MBUFQ_HEAD(mq_freeq) freeq;
	487	struct mbuf m, m_tmp;
	488	u_int32_t c = 0, l = 0;
	489
	490	MBUFQ_INIT(&freeq);
	491
5ba3f43e	492	MBUFQ_FOREACH_SAFE(m, &qmbufq(q), m_tmp) {
316670eb	493	if (flow == 0 \|\| ((m->m_flags & M_PKTHDR) &&
39236c6e	494	m->m_pkthdr.pkt_flowid == flow)) {
316670eb	495	/* remove it from the class queue */
5ba3f43e	496	MBUFQ_REMOVE(&qmbufq(q), m);
316670eb A	497	MBUFQ_NEXT(m) = NULL;
	498
	499	/* and add it to the free queue */
	500	MBUFQ_ENQUEUE(&freeq, m);
	501
	502	l += m_length(m);
	503	c++;
	504	}
	505	}
	506	VERIFY(c == 0 \|\| !MBUFQ_EMPTY(&freeq));
	507
	508	if (c > 0) {
	509	VERIFY(qlen(q) >= c);
	510	qlen(q) -= c;
	511
	512	/* qsize is an approximation, so adjust if necessary */
	513	if (((int)qsize(q) - l) > 0)
	514	qsize(q) -= l;
	515	else if (qsize(q) != 0)
	516	qsize(q) = 0;
	517	}
	518
	519	if (!MBUFQ_EMPTY(&freeq))
	520	m_freem_list(MBUFQ_FIRST(&freeq));
	521
	522	if (cnt != NULL)
	523	*cnt = c;
	524	if (len != NULL)
	525	*len = l;
	526	}
5ba3f43e A	527
	528	void
	529	_flushq_flow(class_queue_t q, u_int32_t flow, u_int32_t cnt, u_int32_t *len)
	530	{
	531	switch (qptype(q)) {
	532	case QP_MBUF:
	533	_flushq_flow_mbuf(q, flow, cnt, len);
	534	break;
	535
	536	default:
	537	VERIFY(0);
	538	/* NOTREACHED */
	539	}
	540	}