#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;
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);
}
qlimit(q) = lim;
qlen(q) = 0;
qsize(q) = 0;
qtype(q) = type;
qptype(q) = ptype;
qstate(q) = QS_RUNNING;
}
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);
}
qlen(q)++;
VERIFY(qlen(q) != 0);
qsize(q) += size;
}
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);
}
qlen(q) += cnt;
qsize(q) += size;
}
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);
}
if (pkt == NULL) {
VERIFY(qlen(q) == 0);
if (qsize(q) > 0)
qsize(q) = 0;
return (NULL);
}
VERIFY(qlen(q) > 0);
qlen(q)--;
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)) {
MBUFQ_REMOVE(&qmbufq(q), m);
MBUFQ_NEXT(m) = NULL;
break;
}
}
if (m != NULL) {
pkt = m;
pkt_len = m_length(m);
}
break;
}
default:
VERIFY(0);
}
if (pkt != NULL) {
VERIFY(qlen(q) > 0);
qlen(q)--;
if (((int)qsize(q) - pkt_len) > 0)
qsize(q) -= pkt_len;
else if (qsize(q) != 0)
qsize(q) = 0;
}
return (pkt);
}
void *
_getq_flow(class_queue_t *q, u_int32_t flow)
{
return (_getq_flow_or_scidx(q, flow, TRUE));
}
void *
_getq_scidx_lt(class_queue_t *q, u_int32_t scidx)
{
return (_getq_flow_or_scidx(q, scidx, FALSE));
}
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);
}
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);
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);
}
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);
}
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;
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);
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);
}
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);
}
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);
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
_removeq(class_queue_t *q, void *pkt)
{
switch (qptype(q)) {
case QP_MBUF:
_removeq_mbuf(q, pkt);
break;
default:
VERIFY(0);
}
}
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)) {
MBUFQ_REMOVE(&qmbufq(q), m);
MBUFQ_NEXT(m) = NULL;
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;
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);
}
}