altq_subr.c

/*-
 * Copyright (C) 1997-2003
 *	Sony Computer Science Laboratories Inc.  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.
 *
 * THIS SOFTWARE IS PROVIDED BY SONY CSL 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 SONY CSL 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.
 *
 * $KAME: altq_subr.c,v 1.21 2003/11/06 06:32:53 kjc Exp $
 */

#include "opt_altq.h"
#include "opt_inet.h"
#include "opt_inet6.h"

#include <sys/param.h>
#include <sys/malloc.h>
#include <sys/mbuf.h>
#include <sys/systm.h>
#include <sys/proc.h>
#include <sys/socket.h>
#include <sys/socketvar.h>
#include <sys/kernel.h>
#include <sys/errno.h>
#include <sys/syslog.h>
#include <sys/sysctl.h>
#include <sys/queue.h>

#include <net/if.h>
#include <net/if_var.h>
#include <net/if_private.h>
#include <net/if_dl.h>
#include <net/if_types.h>
#include <net/vnet.h>

#include <netinet/in.h>
#include <netinet/in_systm.h>
#include <netinet/ip.h>
#ifdef INET6
#include <netinet/ip6.h>
#endif
#include <netinet/tcp.h>
#include <netinet/udp.h>

#include <netpfil/pf/pf.h>
#include <netpfil/pf/pf_altq.h>
#include <net/altq/altq.h>

/* machine dependent clock related includes */
#include <sys/bus.h>
#include <sys/cpu.h>
#include <sys/eventhandler.h>
#include <machine/clock.h>
#if defined(__amd64__) || defined(__i386__)
#include <machine/cpufunc.h>		/* for pentium tsc */
#include <machine/specialreg.h>		/* for CPUID_TSC */
#include <machine/md_var.h>		/* for cpu_feature */
#endif /* __amd64 || __i386__ */

/*
 * internal function prototypes
 */
static void	tbr_timeout(void *);
static struct mbuf *tbr_dequeue(struct ifaltq *, int);
static int tbr_timer = 0;	/* token bucket regulator timer */
static struct callout tbr_callout;

#ifdef ALTQ3_CLFIER_COMPAT
static int 	extract_ports4(struct mbuf *, struct ip *, struct flowinfo_in *);
#ifdef INET6
static int 	extract_ports6(struct mbuf *, struct ip6_hdr *,
			       struct flowinfo_in6 *);
#endif
static int	apply_filter4(u_int32_t, struct flow_filter *,
			      struct flowinfo_in *);
static int	apply_ppfilter4(u_int32_t, struct flow_filter *,
				struct flowinfo_in *);
#ifdef INET6
static int	apply_filter6(u_int32_t, struct flow_filter6 *,
			      struct flowinfo_in6 *);
#endif
static int	apply_tosfilter4(u_int32_t, struct flow_filter *,
				 struct flowinfo_in *);
static u_long	get_filt_handle(struct acc_classifier *, int);
static struct acc_filter *filth_to_filtp(struct acc_classifier *, u_long);
static u_int32_t filt2fibmask(struct flow_filter *);

static void 	ip4f_cache(struct ip *, struct flowinfo_in *);
static int 	ip4f_lookup(struct ip *, struct flowinfo_in *);
static int 	ip4f_init(void);
static struct ip4_frag	*ip4f_alloc(void);
static void 	ip4f_free(struct ip4_frag *);
#endif /* ALTQ3_CLFIER_COMPAT */

#ifdef ALTQ
SYSCTL_NODE(_kern_features, OID_AUTO, altq, CTLFLAG_RD | CTLFLAG_CAPRD, 0,
    "ALTQ packet queuing");

#define	ALTQ_FEATURE(name, desc)					\
	SYSCTL_INT_WITH_LABEL(_kern_features_altq, OID_AUTO, name,	\
	    CTLFLAG_RD | CTLFLAG_CAPRD, SYSCTL_NULL_INT_PTR, 1,		\
	    desc, "feature")

#ifdef ALTQ_CBQ
ALTQ_FEATURE(cbq, "ALTQ Class Based Queuing discipline");
#endif
#ifdef ALTQ_CODEL
ALTQ_FEATURE(codel, "ALTQ Controlled Delay discipline");
#endif
#ifdef ALTQ_RED
ALTQ_FEATURE(red, "ALTQ Random Early Detection discipline");
#endif
#ifdef ALTQ_RIO
ALTQ_FEATURE(rio, "ALTQ Random Early Drop discipline");
#endif
#ifdef ALTQ_HFSC
ALTQ_FEATURE(hfsc, "ALTQ Hierarchical Packet Scheduler discipline");
#endif
#ifdef ALTQ_PRIQ
ALTQ_FEATURE(priq, "ATLQ Priority Queuing discipline");
#endif
#ifdef ALTQ_FAIRQ
ALTQ_FEATURE(fairq, "ALTQ Fair Queuing discipline");
#endif
#endif

/*
 * alternate queueing support routines
 */

/* look up the queue state by the interface name and the queueing type. */
void *
altq_lookup(char *name, int type)
{
	struct ifnet *ifp;

	if ((ifp = ifunit(name)) != NULL) {
		/* read if_snd unlocked */
		if (type != ALTQT_NONE && ifp->if_snd.altq_type == type)
			return (ifp->if_snd.altq_disc);
	}

	return NULL;
}

int
altq_attach(struct ifaltq *ifq, int type, void *discipline,
	int (*enqueue)(struct ifaltq *, struct mbuf *, struct altq_pktattr *),
	struct mbuf *(*dequeue)(struct ifaltq *, int),
	int (*request)(struct ifaltq *, int, void *))
{
	IFQ_LOCK(ifq);
	if (!ALTQ_IS_READY(ifq)) {
		IFQ_UNLOCK(ifq);
		return ENXIO;
	}

	ifq->altq_type     = type;
	ifq->altq_disc     = discipline;
	ifq->altq_enqueue  = enqueue;
	ifq->altq_dequeue  = dequeue;
	ifq->altq_request  = request;
	ifq->altq_flags &= (ALTQF_CANTCHANGE|ALTQF_ENABLED);
	IFQ_UNLOCK(ifq);
	return 0;
}

int
altq_detach(struct ifaltq *ifq)
{
	IFQ_LOCK(ifq);

	if (!ALTQ_IS_READY(ifq)) {
		IFQ_UNLOCK(ifq);
		return ENXIO;
	}
	if (ALTQ_IS_ENABLED(ifq)) {
		IFQ_UNLOCK(ifq);
		return EBUSY;
	}
	if (!ALTQ_IS_ATTACHED(ifq)) {
		IFQ_UNLOCK(ifq);
		return (0);
	}

	ifq->altq_type     = ALTQT_NONE;
	ifq->altq_disc     = NULL;
	ifq->altq_enqueue  = NULL;
	ifq->altq_dequeue  = NULL;
	ifq->altq_request  = NULL;
	ifq->altq_flags &= ALTQF_CANTCHANGE;

	IFQ_UNLOCK(ifq);
	return 0;
}

int
altq_enable(struct ifaltq *ifq)
{
	int s;

	IFQ_LOCK(ifq);

	if (!ALTQ_IS_READY(ifq)) {
		IFQ_UNLOCK(ifq);
		return ENXIO;
	}
	if (ALTQ_IS_ENABLED(ifq)) {
		IFQ_UNLOCK(ifq);
		return 0;
	}

	s = splnet();
	IFQ_PURGE_NOLOCK(ifq);
	ASSERT(ifq->ifq_len == 0);
	ifq->ifq_drv_maxlen = 0;		/* disable bulk dequeue */
	ifq->altq_flags |= ALTQF_ENABLED;
	splx(s);

	IFQ_UNLOCK(ifq);
	return 0;
}

int
altq_disable(struct ifaltq *ifq)
{
	int s;

	IFQ_LOCK(ifq);
	if (!ALTQ_IS_ENABLED(ifq)) {
		IFQ_UNLOCK(ifq);
		return 0;
	}

	s = splnet();
	IFQ_PURGE_NOLOCK(ifq);
	ASSERT(ifq->ifq_len == 0);
	ifq->altq_flags &= ~(ALTQF_ENABLED);
	splx(s);

	IFQ_UNLOCK(ifq);
	return 0;
}

#ifdef ALTQ_DEBUG
void
altq_assert(const char *file, int line, const char *failedexpr)
{
	(void)printf("altq assertion \"%s\" failed: file \"%s\", line %d\n",
		     failedexpr, file, line);
	panic("altq assertion");
	/* NOTREACHED */
}
#endif

/*
 * internal representation of token bucket parameters
 *	rate:	(byte_per_unittime << TBR_SHIFT)  / machclk_freq
 *		(((bits_per_sec) / 8) << TBR_SHIFT) / machclk_freq
 *	depth:	byte << TBR_SHIFT
 *
 */
#define	TBR_SHIFT	29
#define	TBR_SCALE(x)	((int64_t)(x) << TBR_SHIFT)
#define	TBR_UNSCALE(x)	((x) >> TBR_SHIFT)

static struct mbuf *
tbr_dequeue(struct ifaltq *ifq, int op)
{
	struct tb_regulator *tbr;
	struct mbuf *m;
	int64_t interval;
	u_int64_t now;

	IFQ_LOCK_ASSERT(ifq);
	tbr = ifq->altq_tbr;
	if (op == ALTDQ_REMOVE && tbr->tbr_lastop == ALTDQ_POLL) {
		/* if this is a remove after poll, bypass tbr check */
	} else {
		/* update token only when it is negative */
		if (tbr->tbr_token <= 0) {
			now = read_machclk();
			interval = now - tbr->tbr_last;
			if (interval >= tbr->tbr_filluptime)
				tbr->tbr_token = tbr->tbr_depth;
			else {
				tbr->tbr_token += interval * tbr->tbr_rate;
				if (tbr->tbr_token > tbr->tbr_depth)
					tbr->tbr_token = tbr->tbr_depth;
			}
			tbr->tbr_last = now;
		}
		/* if token is still negative, don't allow dequeue */
		if (tbr->tbr_token <= 0)
			return (NULL);
	}

	if (ALTQ_IS_ENABLED(ifq))
		m = (*ifq->altq_dequeue)(ifq, op);
	else {
		if (op == ALTDQ_POLL)
			_IF_POLL(ifq, m);
		else
			_IF_DEQUEUE(ifq, m);
	}

	if (m != NULL && op == ALTDQ_REMOVE)
		tbr->tbr_token -= TBR_SCALE(m_pktlen(m));
	tbr->tbr_lastop = op;
	return (m);
}

/*
 * set a token bucket regulator.
 * if the specified rate is zero, the token bucket regulator is deleted.
 */
int
tbr_set(struct ifaltq *ifq, struct tb_profile *profile)
{
	struct tb_regulator *tbr, *otbr;

	if (tbr_dequeue_ptr == NULL)
		tbr_dequeue_ptr = tbr_dequeue;

	if (machclk_freq == 0)
		init_machclk();
	if (machclk_freq == 0) {
		printf("tbr_set: no cpu clock available!\n");
		return (ENXIO);
	}

	IFQ_LOCK(ifq);
	if (profile->rate == 0) {
		/* delete this tbr */
		if ((tbr = ifq->altq_tbr) == NULL) {
			IFQ_UNLOCK(ifq);
			return (ENOENT);
		}
		ifq->altq_tbr = NULL;
		free(tbr, M_DEVBUF);
		IFQ_UNLOCK(ifq);
		return (0);
	}

	tbr = malloc(sizeof(struct tb_regulator), M_DEVBUF, M_NOWAIT | M_ZERO);
	if (tbr == NULL) {
		IFQ_UNLOCK(ifq);
		return (ENOMEM);
	}

	tbr->tbr_rate = TBR_SCALE(profile->rate / 8) / machclk_freq;
	tbr->tbr_depth = TBR_SCALE(profile->depth);
	if (tbr->tbr_rate > 0)
		tbr->tbr_filluptime = tbr->tbr_depth / tbr->tbr_rate;
	else
		tbr->tbr_filluptime = LLONG_MAX;
	/*
	 *  The longest time between tbr_dequeue() calls will be about 1
	 *  system tick, as the callout that drives it is scheduled once per
	 *  tick.  The refill-time detection logic in tbr_dequeue() can only
	 *  properly detect the passage of up to LLONG_MAX machclk ticks.
	 *  Therefore, in order for this logic to function properly in the
	 *  extreme case, the maximum value of tbr_filluptime should be
	 *  LLONG_MAX less one system tick's worth of machclk ticks less
	 *  some additional slop factor (here one more system tick's worth
	 *  of machclk ticks).
	 */
	if (tbr->tbr_filluptime > (LLONG_MAX - 2 * machclk_per_tick))
		tbr->tbr_filluptime = LLONG_MAX - 2 * machclk_per_tick;
	tbr->tbr_token = tbr->tbr_depth;
	tbr->tbr_last = read_machclk();
	tbr->tbr_lastop = ALTDQ_REMOVE;

	otbr = ifq->altq_tbr;
	ifq->altq_tbr = tbr;	/* set the new tbr */

	if (otbr != NULL)
		free(otbr, M_DEVBUF);
	else {
		if (tbr_timer == 0) {
			CALLOUT_RESET(&tbr_callout, 1, tbr_timeout, (void *)0);
			tbr_timer = 1;
		}
	}
	IFQ_UNLOCK(ifq);
	return (0);
}

/*
 * tbr_timeout goes through the interface list, and kicks the drivers
 * if necessary.
 *
 * MPSAFE
 */
static void
tbr_timeout(void *arg)
{
	VNET_ITERATOR_DECL(vnet_iter);
	struct ifnet *ifp;
	struct epoch_tracker et;
	int active;

	active = 0;
	NET_EPOCH_ENTER(et);
	VNET_LIST_RLOCK_NOSLEEP();
	VNET_FOREACH(vnet_iter) {
		CURVNET_SET(vnet_iter);
		for (ifp = CK_STAILQ_FIRST(&V_ifnet); ifp;
		    ifp = CK_STAILQ_NEXT(ifp, if_link)) {
			/* read from if_snd unlocked */
			if (!TBR_IS_ENABLED(&ifp->if_snd))
				continue;
			active++;
			if (!IFQ_IS_EMPTY(&ifp->if_snd) &&
			    ifp->if_start != NULL)
				(*ifp->if_start)(ifp);
		}
		CURVNET_RESTORE();
	}
	VNET_LIST_RUNLOCK_NOSLEEP();
	NET_EPOCH_EXIT(et);
	if (active > 0)
		CALLOUT_RESET(&tbr_callout, 1, tbr_timeout, (void *)0);
	else
		tbr_timer = 0;	/* don't need tbr_timer anymore */
}

/*
 * attach a discipline to the interface.  if one already exists, it is
 * overridden.
 * Locking is done in the discipline specific attach functions. Basically
 * they call back to altq_attach which takes care of the attach and locking.
 */
int
altq_pfattach(struct pf_altq *a)
{
	int error = 0;

	switch (a->scheduler) {
	case ALTQT_NONE:
		break;
#ifdef ALTQ_CBQ
	case ALTQT_CBQ:
		error = cbq_pfattach(a);
		break;
#endif
#ifdef ALTQ_PRIQ
	case ALTQT_PRIQ:
		error = priq_pfattach(a);
		break;
#endif
#ifdef ALTQ_HFSC
	case ALTQT_HFSC:
		error = hfsc_pfattach(a);
		break;
#endif
#ifdef ALTQ_FAIRQ
	case ALTQT_FAIRQ:
		error = fairq_pfattach(a);
		break;
#endif
#ifdef ALTQ_CODEL
	case ALTQT_CODEL:
		error = codel_pfattach(a);
		break;
#endif
	default:
		error = ENXIO;
	}

	return (error);
}

/*
 * detach a discipline from the interface.
 * it is possible that the discipline was already overridden by another
 * discipline.
 */
int
altq_pfdetach(struct pf_altq *a)
{
	struct ifnet *ifp;
	int s, error = 0;

	if ((ifp = ifunit(a->ifname)) == NULL)
		return (EINVAL);

	/* if this discipline is no longer referenced, just return */
	/* read unlocked from if_snd */
	if (a->altq_disc == NULL || a->altq_disc != ifp->if_snd.altq_disc)
		return (0);

	s = splnet();
	/* read unlocked from if_snd, _disable and _detach take care */
	if (ALTQ_IS_ENABLED(&ifp->if_snd))
		error = altq_disable(&ifp->if_snd);
	if (error == 0)
		error = altq_detach(&ifp->if_snd);
	splx(s);

	return (error);
}

/*
 * add a discipline or a queue
 * Locking is done in the discipline specific functions with regards to
 * malloc with WAITOK, also it is not yet clear which lock to use.
 */
int
altq_add(struct ifnet *ifp, struct pf_altq *a)
{
	int error = 0;

	if (a->qname[0] != 0)
		return (altq_add_queue(a));

	if (machclk_freq == 0)
		init_machclk();
	if (machclk_freq == 0)
		panic("altq_add: no cpu clock");

	switch (a->scheduler) {
#ifdef ALTQ_CBQ
	case ALTQT_CBQ:
		error = cbq_add_altq(ifp, a);
		break;
#endif
#ifdef ALTQ_PRIQ
	case ALTQT_PRIQ:
		error = priq_add_altq(ifp, a);
		break;
#endif
#ifdef ALTQ_HFSC
	case ALTQT_HFSC:
		error = hfsc_add_altq(ifp, a);
		break;
#endif
#ifdef ALTQ_FAIRQ
        case ALTQT_FAIRQ:
                error = fairq_add_altq(ifp, a);
                break;
#endif
#ifdef ALTQ_CODEL
	case ALTQT_CODEL:
		error = codel_add_altq(ifp, a);
		break;
#endif
	default:
		error = ENXIO;
	}

	return (error);
}

/*
 * remove a discipline or a queue
 * It is yet unclear what lock to use to protect this operation, the
 * discipline specific functions will determine and grab it
 */
int
altq_remove(struct pf_altq *a)
{
	int error = 0;

	if (a->qname[0] != 0)
		return (altq_remove_queue(a));

	switch (a->scheduler) {
#ifdef ALTQ_CBQ
	case ALTQT_CBQ:
		error = cbq_remove_altq(a);
		break;
#endif
#ifdef ALTQ_PRIQ
	case ALTQT_PRIQ:
		error = priq_remove_altq(a);
		break;
#endif
#ifdef ALTQ_HFSC
	case ALTQT_HFSC:
		error = hfsc_remove_altq(a);
		break;
#endif
#ifdef ALTQ_FAIRQ
        case ALTQT_FAIRQ:
                error = fairq_remove_altq(a);
                break;
#endif
#ifdef ALTQ_CODEL
	case ALTQT_CODEL:
		error = codel_remove_altq(a);
		break;
#endif
	default:
		error = ENXIO;
	}

	return (error);
}

/*
 * add a queue to the discipline
 * It is yet unclear what lock to use to protect this operation, the
 * discipline specific functions will determine and grab it
 */
int
altq_add_queue(struct pf_altq *a)
{
	int error = 0;

	switch (a->scheduler) {
#ifdef ALTQ_CBQ
	case ALTQT_CBQ:
		error = cbq_add_queue(a);
		break;
#endif
#ifdef ALTQ_PRIQ
	case ALTQT_PRIQ:
		error = priq_add_queue(a);
		break;
#endif
#ifdef ALTQ_HFSC
	case ALTQT_HFSC:
		error = hfsc_add_queue(a);
		break;
#endif
#ifdef ALTQ_FAIRQ
        case ALTQT_FAIRQ:
                error = fairq_add_queue(a);
                break;
#endif
	default:
		error = ENXIO;
	}

	return (error);
}

/*
 * remove a queue from the discipline
 * It is yet unclear what lock to use to protect this operation, the
 * discipline specific functions will determine and grab it
 */
int
altq_remove_queue(struct pf_altq *a)
{
	int error = 0;

	switch (a->scheduler) {
#ifdef ALTQ_CBQ
	case ALTQT_CBQ:
		error = cbq_remove_queue(a);
		break;
#endif
#ifdef ALTQ_PRIQ
	case ALTQT_PRIQ:
		error = priq_remove_queue(a);
		break;
#endif
#ifdef ALTQ_HFSC
	case ALTQT_HFSC:
		error = hfsc_remove_queue(a);
		break;
#endif
#ifdef ALTQ_FAIRQ
        case ALTQT_FAIRQ:
                error = fairq_remove_queue(a);
                break;
#endif
	default:
		error = ENXIO;
	}

	return (error);
}

/*
 * get queue statistics
 * Locking is done in the discipline specific functions with regards to
 * copyout operations, also it is not yet clear which lock to use.
 */
int
altq_getqstats(struct pf_altq *a, void *ubuf, int *nbytes, int version)
{
	int error = 0;

	switch (a->scheduler) {
#ifdef ALTQ_CBQ
	case ALTQT_CBQ:
		error = cbq_getqstats(a, ubuf, nbytes, version);
		break;
#endif
#ifdef ALTQ_PRIQ
	case ALTQT_PRIQ:
		error = priq_getqstats(a, ubuf, nbytes, version);
		break;
#endif
#ifdef ALTQ_HFSC
	case ALTQT_HFSC:
		error = hfsc_getqstats(a, ubuf, nbytes, version);
		break;
#endif
#ifdef ALTQ_FAIRQ
        case ALTQT_FAIRQ:
                error = fairq_getqstats(a, ubuf, nbytes, version);
                break;
#endif
#ifdef ALTQ_CODEL
	case ALTQT_CODEL:
		error = codel_getqstats(a, ubuf, nbytes, version);
		break;
#endif
	default:
		error = ENXIO;
	}

	return (error);
}

/*
 * read and write diffserv field in IPv4 or IPv6 header
 */
u_int8_t
read_dsfield(struct mbuf *m, struct altq_pktattr *pktattr)
{
	struct mbuf *m0;
	u_int8_t ds_field = 0;

	if (pktattr == NULL ||
	    (pktattr->pattr_af != AF_INET && pktattr->pattr_af != AF_INET6))
		return ((u_int8_t)0);

	/* verify that pattr_hdr is within the mbuf data */
	for (m0 = m; m0 != NULL; m0 = m0->m_next)
		if ((pktattr->pattr_hdr >= m0->m_data) &&
		    (pktattr->pattr_hdr < m0->m_data + m0->m_len))
			break;
	if (m0 == NULL) {
		/* ick, pattr_hdr is stale */
		pktattr->pattr_af = AF_UNSPEC;
#ifdef ALTQ_DEBUG
		printf("read_dsfield: can't locate header!\n");
#endif
		return ((u_int8_t)0);
	}

	if (pktattr->pattr_af == AF_INET) {
		struct ip *ip = (struct ip *)pktattr->pattr_hdr;

		if (ip->ip_v != 4)
			return ((u_int8_t)0);	/* version mismatch! */
		ds_field = ip->ip_tos;
	}
#ifdef INET6
	else if (pktattr->pattr_af == AF_INET6) {
		struct ip6_hdr *ip6 = (struct ip6_hdr *)pktattr->pattr_hdr;
		u_int32_t flowlabel;

		flowlabel = ntohl(ip6->ip6_flow);
		if ((flowlabel >> 28) != 6)
			return ((u_int8_t)0);	/* version mismatch! */
		ds_field = (flowlabel >> 20) & 0xff;
	}
#endif
	return (ds_field);
}

void
write_dsfield(struct mbuf *m, struct altq_pktattr *pktattr, u_int8_t dsfield)
{
	struct mbuf *m0;

	if (pktattr == NULL ||
	    (pktattr->pattr_af != AF_INET && pktattr->pattr_af != AF_INET6))
		return;

	/* verify that pattr_hdr is within the mbuf data */
	for (m0 = m; m0 != NULL; m0 = m0->m_next)
		if ((pktattr->pattr_hdr >= m0->m_data) &&
		    (pktattr->pattr_hdr < m0->m_data + m0->m_len))
			break;
	if (m0 == NULL) {
		/* ick, pattr_hdr is stale */
		pktattr->pattr_af = AF_UNSPEC;
#ifdef ALTQ_DEBUG
		printf("write_dsfield: can't locate header!\n");
#endif
		return;
	}

	if (pktattr->pattr_af == AF_INET) {
		struct ip *ip = (struct ip *)pktattr->pattr_hdr;
		u_int8_t old;
		int32_t sum;

		if (ip->ip_v != 4)
			return;		/* version mismatch! */
		old = ip->ip_tos;
		dsfield |= old & 3;	/* leave CU bits */
		if (old == dsfield)
			return;
		ip->ip_tos = dsfield;
		/*
		 * update checksum (from RFC1624)
		 *	   HC' = ~(~HC + ~m + m')
		 */
		sum = ~ntohs(ip->ip_sum) & 0xffff;
		sum += 0xff00 + (~old & 0xff) + dsfield;
		sum = (sum >> 16) + (sum & 0xffff);
		sum += (sum >> 16);  /* add carry */

		ip->ip_sum = htons(~sum & 0xffff);
	}
#ifdef INET6
	else if (pktattr->pattr_af == AF_INET6) {
		struct ip6_hdr *ip6 = (struct ip6_hdr *)pktattr->pattr_hdr;
		u_int32_t flowlabel;

		flowlabel = ntohl(ip6->ip6_flow);
		if ((flowlabel >> 28) != 6)
			return;		/* version mismatch! */
		flowlabel = (flowlabel & 0xf03fffff) | (dsfield << 20);
		ip6->ip6_flow = htonl(flowlabel);
	}
#endif
	return;
}

/*
 * high resolution clock support taking advantage of a machine dependent
 * high resolution time counter (e.g., timestamp counter of intel pentium).
 * we assume
 *  - 64-bit-long monotonically-increasing counter
 *  - frequency range is 100M-4GHz (CPU speed)
 */
/* if pcc is not available or disabled, emulate 256MHz using microtime() */
#define	MACHCLK_SHIFT	8

int machclk_usepcc;
u_int32_t machclk_freq;
u_int32_t machclk_per_tick;

#if defined(__i386__) && defined(__NetBSD__)
extern u_int64_t cpu_tsc_freq;
#endif

/* Update TSC freq with the value indicated by the caller. */
static void
tsc_freq_changed(void *arg, const struct cf_level *level, int status)
{
	/* If there was an error during the transition, don't do anything. */
	if (status != 0)
		return;

#if defined(__amd64__) || defined(__i386__)
	/* If TSC is P-state invariant, don't do anything. */
	if (tsc_is_invariant)
		return;
#endif

	/* Total setting for this level gives the new frequency in MHz. */
	init_machclk();
}
EVENTHANDLER_DEFINE(cpufreq_post_change, tsc_freq_changed, NULL,
    EVENTHANDLER_PRI_LAST);

static void
init_machclk_setup(void)
{
	callout_init(&tbr_callout, 1);

	machclk_usepcc = 1;

#if (!defined(__amd64__) && !defined(__i386__)) || defined(ALTQ_NOPCC)
	machclk_usepcc = 0;
#endif
#if defined(__FreeBSD__) && defined(SMP)
	machclk_usepcc = 0;
#endif
#if defined(__NetBSD__) && defined(MULTIPROCESSOR)
	machclk_usepcc = 0;
#endif
#if defined(__amd64__) || defined(__i386__)
	/* check if TSC is available */
	if ((cpu_feature & CPUID_TSC) == 0 ||
	    atomic_load_acq_64(&tsc_freq) == 0)
		machclk_usepcc = 0;
#endif
}

void
init_machclk(void)
{
	static int called;

	/* Call one-time initialization function. */
	if (!called) {
		init_machclk_setup();
		called = 1;
	}

	if (machclk_usepcc == 0) {
		/* emulate 256MHz using microtime() */
		machclk_freq = 1000000 << MACHCLK_SHIFT;
		machclk_per_tick = machclk_freq / hz;
#ifdef ALTQ_DEBUG
		printf("altq: emulate %uHz cpu clock\n", machclk_freq);
#endif
		return;
	}

	/*
	 * if the clock frequency (of Pentium TSC or Alpha PCC) is
	 * accessible, just use it.
	 */
#if defined(__amd64__) || defined(__i386__)
	machclk_freq = atomic_load_acq_64(&tsc_freq);
#endif

	/*
	 * if we don't know the clock frequency, measure it.
	 */
	if (machclk_freq == 0) {
		static int	wait;
		struct timeval	tv_start, tv_end;
		u_int64_t	start, end, diff;
		int		timo;

		microtime(&tv_start);
		start = read_machclk();
		timo = hz;	/* 1 sec */
		(void)tsleep(&wait, PWAIT | PCATCH, "init_machclk", timo);
		microtime(&tv_end);
		end = read_machclk();
		diff = (u_int64_t)(tv_end.tv_sec - tv_start.tv_sec) * 1000000
		    + tv_end.tv_usec - tv_start.tv_usec;
		if (diff != 0)
			machclk_freq = (u_int)((end - start) * 1000000 / diff);
	}

	machclk_per_tick = machclk_freq / hz;

#ifdef ALTQ_DEBUG
	printf("altq: CPU clock: %uHz\n", machclk_freq);
#endif
}

#if defined(__OpenBSD__) && defined(__i386__)
static __inline u_int64_t
rdtsc(void)
{
	u_int64_t rv;
	__asm __volatile(".byte 0x0f, 0x31" : "=A" (rv));
	return (rv);
}
#endif /* __OpenBSD__ && __i386__ */

u_int64_t
read_machclk(void)
{
	u_int64_t val;

	if (machclk_usepcc) {
#if defined(__amd64__) || defined(__i386__)
		val = rdtsc();
#else
		panic("read_machclk");
#endif
	} else {
		struct timeval tv, boottime;

		microtime(&tv);
		getboottime(&boottime);
		val = (((u_int64_t)(tv.tv_sec - boottime.tv_sec) * 1000000
		    + tv.tv_usec) << MACHCLK_SHIFT);
	}
	return (val);
}

#ifdef ALTQ3_CLFIER_COMPAT

#ifndef IPPROTO_ESP
#define	IPPROTO_ESP	50		/* encapsulating security payload */
#endif
#ifndef IPPROTO_AH
#define	IPPROTO_AH	51		/* authentication header */
#endif

/*
 * extract flow information from a given packet.
 * filt_mask shows flowinfo fields required.
 * we assume the ip header is in one mbuf, and addresses and ports are
 * in network byte order.
 */
int
altq_extractflow(m, af, flow, filt_bmask)
	struct mbuf *m;
	int af;
	struct flowinfo *flow;
	u_int32_t	filt_bmask;
{

	switch (af) {
	case PF_INET: {
		struct flowinfo_in *fin;
		struct ip *ip;

		ip = mtod(m, struct ip *);

		if (ip->ip_v != 4)
			break;

		fin = (struct flowinfo_in *)flow;
		fin->fi_len = sizeof(struct flowinfo_in);
		fin->fi_family = AF_INET;

		fin->fi_proto = ip->ip_p;
		fin->fi_tos = ip->ip_tos;

		fin->fi_src.s_addr = ip->ip_src.s_addr;
		fin->fi_dst.s_addr = ip->ip_dst.s_addr;

		if (filt_bmask & FIMB4_PORTS)
			/* if port info is required, extract port numbers */
			extract_ports4(m, ip, fin);
		else {
			fin->fi_sport = 0;
			fin->fi_dport = 0;
			fin->fi_gpi = 0;
		}
		return (1);
	}

#ifdef INET6
	case PF_INET6: {
		struct flowinfo_in6 *fin6;
		struct ip6_hdr *ip6;

		ip6 = mtod(m, struct ip6_hdr *);
		/* should we check the ip version? */

		fin6 = (struct flowinfo_in6 *)flow;
		fin6->fi6_len = sizeof(struct flowinfo_in6);
		fin6->fi6_family = AF_INET6;

		fin6->fi6_proto = ip6->ip6_nxt;
		fin6->fi6_tclass   = IPV6_TRAFFIC_CLASS(ip6);

		fin6->fi6_flowlabel = ip6->ip6_flow & htonl(0x000fffff);
		fin6->fi6_src = ip6->ip6_src;
		fin6->fi6_dst = ip6->ip6_dst;

		if ((filt_bmask & FIMB6_PORTS) ||
		    ((filt_bmask & FIMB6_PROTO)
		     && ip6->ip6_nxt > IPPROTO_IPV6))
			/*
			 * if port info is required, or proto is required
			 * but there are option headers, extract port
			 * and protocol numbers.
			 */
			extract_ports6(m, ip6, fin6);
		else {
			fin6->fi6_sport = 0;
			fin6->fi6_dport = 0;
			fin6->fi6_gpi = 0;
		}
		return (1);
	}
#endif /* INET6 */

	default:
		break;
	}

	/* failed */
	flow->fi_len = sizeof(struct flowinfo);
	flow->fi_family = AF_UNSPEC;
	return (0);
}

/*
 * helper routine to extract port numbers
 */
/* structure for ipsec and ipv6 option header template */
struct _opt6 {
	u_int8_t	opt6_nxt;	/* next header */
	u_int8_t	opt6_hlen;	/* header extension length */
	u_int16_t	_pad;
	u_int32_t	ah_spi;		/* security parameter index
					   for authentication header */
};

/*
 * extract port numbers from a ipv4 packet.
 */
static int
extract_ports4(m, ip, fin)
	struct mbuf *m;
	struct ip *ip;
	struct flowinfo_in *fin;
{
	struct mbuf *m0;
	u_short ip_off;
	u_int8_t proto;
	int 	off;

	fin->fi_sport = 0;
	fin->fi_dport = 0;
	fin->fi_gpi = 0;

	ip_off = ntohs(ip->ip_off);
	/* if it is a fragment, try cached fragment info */
	if (ip_off & IP_OFFMASK) {
		ip4f_lookup(ip, fin);
		return (1);
	}

	/* locate the mbuf containing the protocol header */
	for (m0 = m; m0 != NULL; m0 = m0->m_next)
		if (((caddr_t)ip >= m0->m_data) &&
		    ((caddr_t)ip < m0->m_data + m0->m_len))
			break;
	if (m0 == NULL) {
#ifdef ALTQ_DEBUG
		printf("extract_ports4: can't locate header! ip=%p\n", ip);
#endif
		return (0);
	}
	off = ((caddr_t)ip - m0->m_data) + (ip->ip_hl << 2);
	proto = ip->ip_p;

#ifdef ALTQ_IPSEC
 again:
#endif
	while (off >= m0->m_len) {
		off -= m0->m_len;
		m0 = m0->m_next;
		if (m0 == NULL)
			return (0);  /* bogus ip_hl! */
	}
	if (m0->m_len < off + 4)
		return (0);

	switch (proto) {
	case IPPROTO_TCP:
	case IPPROTO_UDP: {
		struct udphdr *udp;

		udp = (struct udphdr *)(mtod(m0, caddr_t) + off);
		fin->fi_sport = udp->uh_sport;
		fin->fi_dport = udp->uh_dport;
		fin->fi_proto = proto;
		}
		break;

#ifdef ALTQ_IPSEC
	case IPPROTO_ESP:
		if (fin->fi_gpi == 0){
			u_int32_t *gpi;

			gpi = (u_int32_t *)(mtod(m0, caddr_t) + off);
			fin->fi_gpi   = *gpi;
		}
		fin->fi_proto = proto;
		break;

	case IPPROTO_AH: {
			/* get next header and header length */
			struct _opt6 *opt6;

			opt6 = (struct _opt6 *)(mtod(m0, caddr_t) + off);
			proto = opt6->opt6_nxt;
			off += 8 + (opt6->opt6_hlen * 4);
			if (fin->fi_gpi == 0 && m0->m_len >= off + 8)
				fin->fi_gpi = opt6->ah_spi;
		}
		/* goto the next header */
		goto again;
#endif  /* ALTQ_IPSEC */

	default:
		fin->fi_proto = proto;
		return (0);
	}

	/* if this is a first fragment, cache it. */
	if (ip_off & IP_MF)
		ip4f_cache(ip, fin);

	return (1);
}

#ifdef INET6
static int
extract_ports6(m, ip6, fin6)
	struct mbuf *m;
	struct ip6_hdr *ip6;
	struct flowinfo_in6 *fin6;
{
	struct mbuf *m0;
	int	off;
	u_int8_t proto;

	fin6->fi6_gpi   = 0;
	fin6->fi6_sport = 0;
	fin6->fi6_dport = 0;

	/* locate the mbuf containing the protocol header */
	for (m0 = m; m0 != NULL; m0 = m0->m_next)
		if (((caddr_t)ip6 >= m0->m_data) &&
		    ((caddr_t)ip6 < m0->m_data + m0->m_len))
			break;
	if (m0 == NULL) {
#ifdef ALTQ_DEBUG
		printf("extract_ports6: can't locate header! ip6=%p\n", ip6);
#endif
		return (0);
	}
	off = ((caddr_t)ip6 - m0->m_data) + sizeof(struct ip6_hdr);

	proto = ip6->ip6_nxt;
	do {
		while (off >= m0->m_len) {
			off -= m0->m_len;
			m0 = m0->m_next;
			if (m0 == NULL)
				return (0);
		}
		if (m0->m_len < off + 4)
			return (0);

		switch (proto) {
		case IPPROTO_TCP:
		case IPPROTO_UDP: {
			struct udphdr *udp;

			udp = (struct udphdr *)(mtod(m0, caddr_t) + off);
			fin6->fi6_sport = udp->uh_sport;
			fin6->fi6_dport = udp->uh_dport;
			fin6->fi6_proto = proto;
			}
			return (1);

		case IPPROTO_ESP:
			if (fin6->fi6_gpi == 0) {
				u_int32_t *gpi;

				gpi = (u_int32_t *)(mtod(m0, caddr_t) + off);
				fin6->fi6_gpi   = *gpi;
			}
			fin6->fi6_proto = proto;
			return (1);

		case IPPROTO_AH: {
			/* get next header and header length */
			struct _opt6 *opt6;

			opt6 = (struct _opt6 *)(mtod(m0, caddr_t) + off);
			if (fin6->fi6_gpi == 0 && m0->m_len >= off + 8)
				fin6->fi6_gpi = opt6->ah_spi;
			proto = opt6->opt6_nxt;
			off += 8 + (opt6->opt6_hlen * 4);
			/* goto the next header */
			break;
			}

		case IPPROTO_HOPOPTS:
		case IPPROTO_ROUTING:
		case IPPROTO_DSTOPTS: {
			/* get next header and header length */
			struct _opt6 *opt6;

			opt6 = (struct _opt6 *)(mtod(m0, caddr_t) + off);
			proto = opt6->opt6_nxt;
			off += (opt6->opt6_hlen + 1) * 8;
			/* goto the next header */
			break;
			}

		case IPPROTO_FRAGMENT:
			/* ipv6 fragmentations are not supported yet */
		default:
			fin6->fi6_proto = proto;
			return (0);
		}
	} while (1);
	/*NOTREACHED*/
}
#endif /* INET6 */

/*
 * altq common classifier
 */
int
acc_add_filter(classifier, filter, class, phandle)
	struct acc_classifier *classifier;
	struct flow_filter *filter;
	void	*class;
	u_long	*phandle;
{
	struct acc_filter *afp, *prev, *tmp;
	int	i, s;

#ifdef INET6
	if (filter->ff_flow.fi_family != AF_INET &&
	    filter->ff_flow.fi_family != AF_INET6)
		return (EINVAL);
#else
	if (filter->ff_flow.fi_family != AF_INET)
		return (EINVAL);
#endif

	afp = malloc(sizeof(*afp), M_DEVBUF, M_WAITOK | M_ZERO);
	afp->f_filter = *filter;
	afp->f_class = class;

	i = ACC_WILDCARD_INDEX;
	if (filter->ff_flow.fi_family == AF_INET) {
		struct flow_filter *filter4 = &afp->f_filter;

		/*
		 * if address is 0, it's a wildcard.  if address mask
		 * isn't set, use full mask.
		 */
		if (filter4->ff_flow.fi_dst.s_addr == 0)
			filter4->ff_mask.mask_dst.s_addr = 0;
		else if (filter4->ff_mask.mask_dst.s_addr == 0)
			filter4->ff_mask.mask_dst.s_addr = 0xffffffff;
		if (filter4->ff_flow.fi_src.s_addr == 0)
			filter4->ff_mask.mask_src.s_addr = 0;
		else if (filter4->ff_mask.mask_src.s_addr == 0)
			filter4->ff_mask.mask_src.s_addr = 0xffffffff;

		/* clear extra bits in addresses  */
		   filter4->ff_flow.fi_dst.s_addr &=
		       filter4->ff_mask.mask_dst.s_addr;
		   filter4->ff_flow.fi_src.s_addr &=
		       filter4->ff_mask.mask_src.s_addr;

		/*
		 * if dst address is a wildcard, use hash-entry
		 * ACC_WILDCARD_INDEX.
		 */
		if (filter4->ff_mask.mask_dst.s_addr != 0xffffffff)
			i = ACC_WILDCARD_INDEX;
		else
			i = ACC_GET_HASH_INDEX(filter4->ff_flow.fi_dst.s_addr);
	}
#ifdef INET6
	else if (filter->ff_flow.fi_family == AF_INET6) {
		struct flow_filter6 *filter6 =
			(struct flow_filter6 *)&afp->f_filter;
#ifndef IN6MASK0 /* taken from kame ipv6 */
#define	IN6MASK0	{{{ 0, 0, 0, 0 }}}
#define	IN6MASK128	{{{ 0xffffffff, 0xffffffff, 0xffffffff, 0xffffffff }}}
		const struct in6_addr in6mask0 = IN6MASK0;
		const struct in6_addr in6mask128 = IN6MASK128;
#endif

		if (IN6_IS_ADDR_UNSPECIFIED(&filter6->ff_flow6.fi6_dst))
			filter6->ff_mask6.mask6_dst = in6mask0;
		else if (IN6_IS_ADDR_UNSPECIFIED(&filter6->ff_mask6.mask6_dst))
			filter6->ff_mask6.mask6_dst = in6mask128;
		if (IN6_IS_ADDR_UNSPECIFIED(&filter6->ff_flow6.fi6_src))
			filter6->ff_mask6.mask6_src = in6mask0;
		else if (IN6_IS_ADDR_UNSPECIFIED(&filter6->ff_mask6.mask6_src))
			filter6->ff_mask6.mask6_src = in6mask128;

		/* clear extra bits in addresses  */
		for (i = 0; i < 16; i++)
			filter6->ff_flow6.fi6_dst.s6_addr[i] &=
			    filter6->ff_mask6.mask6_dst.s6_addr[i];
		for (i = 0; i < 16; i++)
			filter6->ff_flow6.fi6_src.s6_addr[i] &=
			    filter6->ff_mask6.mask6_src.s6_addr[i];

		if (filter6->ff_flow6.fi6_flowlabel == 0)
			i = ACC_WILDCARD_INDEX;
		else
			i = ACC_GET_HASH_INDEX(filter6->ff_flow6.fi6_flowlabel);
	}
#endif /* INET6 */

	afp->f_handle = get_filt_handle(classifier, i);

	/* update filter bitmask */
	afp->f_fbmask = filt2fibmask(filter);
	classifier->acc_fbmask |= afp->f_fbmask;

	/*
	 * add this filter to the filter list.
	 * filters are ordered from the highest rule number.
	 */
	s = splnet();
	prev = NULL;
	LIST_FOREACH(tmp, &classifier->acc_filters[i], f_chain) {
		if (tmp->f_filter.ff_ruleno > afp->f_filter.ff_ruleno)
			prev = tmp;
		else
			break;
	}
	if (prev == NULL)
		LIST_INSERT_HEAD(&classifier->acc_filters[i], afp, f_chain);
	else
		LIST_INSERT_AFTER(prev, afp, f_chain);
	splx(s);

	*phandle = afp->f_handle;
	return (0);
}

int
acc_delete_filter(classifier, handle)
	struct acc_classifier *classifier;
	u_long handle;
{
	struct acc_filter *afp;
	int	s;

	if ((afp = filth_to_filtp(classifier, handle)) == NULL)
		return (EINVAL);

	s = splnet();
	LIST_REMOVE(afp, f_chain);
	splx(s);

	free(afp, M_DEVBUF);

	/* todo: update filt_bmask */

	return (0);
}

/*
 * delete filters referencing to the specified class.
 * if the all flag is not 0, delete all the filters.
 */
int
acc_discard_filters(classifier, class, all)
	struct acc_classifier *classifier;
	void	*class;
	int	all;
{
	struct acc_filter *afp;
	int	i, s;

	s = splnet();
	for (i = 0; i < ACC_FILTER_TABLESIZE; i++) {
		do {
			LIST_FOREACH(afp, &classifier->acc_filters[i], f_chain)
				if (all || afp->f_class == class) {
					LIST_REMOVE(afp, f_chain);
					free(afp, M_DEVBUF);
					/* start again from the head */
					break;
				}
		} while (afp != NULL);
	}
	splx(s);

	if (all)
		classifier->acc_fbmask = 0;

	return (0);
}

void *
acc_classify(clfier, m, af)
	void *clfier;
	struct mbuf *m;
	int af;
{
	struct acc_classifier *classifier;
	struct flowinfo flow;
	struct acc_filter *afp;
	int	i;

	classifier = (struct acc_classifier *)clfier;
	altq_extractflow(m, af, &flow, classifier->acc_fbmask);

	if (flow.fi_family == AF_INET) {
		struct flowinfo_in *fp = (struct flowinfo_in *)&flow;

		if ((classifier->acc_fbmask & FIMB4_ALL) == FIMB4_TOS) {
			/* only tos is used */
			LIST_FOREACH(afp,
				 &classifier->acc_filters[ACC_WILDCARD_INDEX],
				 f_chain)
				if (apply_tosfilter4(afp->f_fbmask,
						     &afp->f_filter, fp))
					/* filter matched */
					return (afp->f_class);
		} else if ((classifier->acc_fbmask &
			(~(FIMB4_PROTO|FIMB4_SPORT|FIMB4_DPORT) & FIMB4_ALL))
		    == 0) {
			/* only proto and ports are used */
			LIST_FOREACH(afp,
				 &classifier->acc_filters[ACC_WILDCARD_INDEX],
				 f_chain)
				if (apply_ppfilter4(afp->f_fbmask,
						    &afp->f_filter, fp))
					/* filter matched */
					return (afp->f_class);
		} else {
			/* get the filter hash entry from its dest address */
			i = ACC_GET_HASH_INDEX(fp->fi_dst.s_addr);
			do {
				/*
				 * go through this loop twice.  first for dst
				 * hash, second for wildcards.
				 */
				LIST_FOREACH(afp, &classifier->acc_filters[i],
					     f_chain)
					if (apply_filter4(afp->f_fbmask,
							  &afp->f_filter, fp))
						/* filter matched */
						return (afp->f_class);

				/*
				 * check again for filters with a dst addr
				 * wildcard.
				 * (daddr == 0 || dmask != 0xffffffff).
				 */
				if (i != ACC_WILDCARD_INDEX)
					i = ACC_WILDCARD_INDEX;
				else
					break;
			} while (1);
		}
	}
#ifdef INET6
	else if (flow.fi_family == AF_INET6) {
		struct flowinfo_in6 *fp6 = (struct flowinfo_in6 *)&flow;

		/* get the filter hash entry from its flow ID */
		if (fp6->fi6_flowlabel != 0)
			i = ACC_GET_HASH_INDEX(fp6->fi6_flowlabel);
		else
			/* flowlable can be zero */
			i = ACC_WILDCARD_INDEX;

		/* go through this loop twice.  first for flow hash, second
		   for wildcards. */
		do {
			LIST_FOREACH(afp, &classifier->acc_filters[i], f_chain)
				if (apply_filter6(afp->f_fbmask,
					(struct flow_filter6 *)&afp->f_filter,
					fp6))
					/* filter matched */
					return (afp->f_class);

			/*
			 * check again for filters with a wildcard.
			 */
			if (i != ACC_WILDCARD_INDEX)
				i = ACC_WILDCARD_INDEX;
			else
				break;
		} while (1);
	}
#endif /* INET6 */

	/* no filter matched */
	return (NULL);
}

static int
apply_filter4(fbmask, filt, pkt)
	u_int32_t	fbmask;
	struct flow_filter *filt;
	struct flowinfo_in *pkt;
{
	if (filt->ff_flow.fi_family != AF_INET)
		return (0);
	if ((fbmask & FIMB4_SPORT) && filt->ff_flow.fi_sport != pkt->fi_sport)
		return (0);
	if ((fbmask & FIMB4_DPORT) && filt->ff_flow.fi_dport != pkt->fi_dport)
		return (0);
	if ((fbmask & FIMB4_DADDR) &&
	    filt->ff_flow.fi_dst.s_addr !=
	    (pkt->fi_dst.s_addr & filt->ff_mask.mask_dst.s_addr))
		return (0);
	if ((fbmask & FIMB4_SADDR) &&
	    filt->ff_flow.fi_src.s_addr !=
	    (pkt->fi_src.s_addr & filt->ff_mask.mask_src.s_addr))
		return (0);
	if ((fbmask & FIMB4_PROTO) && filt->ff_flow.fi_proto != pkt->fi_proto)
		return (0);
	if ((fbmask & FIMB4_TOS) && filt->ff_flow.fi_tos !=
	    (pkt->fi_tos & filt->ff_mask.mask_tos))
		return (0);
	if ((fbmask & FIMB4_GPI) && filt->ff_flow.fi_gpi != (pkt->fi_gpi))
		return (0);
	/* match */
	return (1);
}

/*
 * filter matching function optimized for a common case that checks
 * only protocol and port numbers
 */
static int
apply_ppfilter4(fbmask, filt, pkt)
	u_int32_t	fbmask;
	struct flow_filter *filt;
	struct flowinfo_in *pkt;
{
	if (filt->ff_flow.fi_family != AF_INET)
		return (0);
	if ((fbmask & FIMB4_SPORT) && filt->ff_flow.fi_sport != pkt->fi_sport)
		return (0);
	if ((fbmask & FIMB4_DPORT) && filt->ff_flow.fi_dport != pkt->fi_dport)
		return (0);
	if ((fbmask & FIMB4_PROTO) && filt->ff_flow.fi_proto != pkt->fi_proto)
		return (0);
	/* match */
	return (1);
}

/*
 * filter matching function only for tos field.
 */
static int
apply_tosfilter4(fbmask, filt, pkt)
	u_int32_t	fbmask;
	struct flow_filter *filt;
	struct flowinfo_in *pkt;
{
	if (filt->ff_flow.fi_family != AF_INET)
		return (0);
	if ((fbmask & FIMB4_TOS) && filt->ff_flow.fi_tos !=
	    (pkt->fi_tos & filt->ff_mask.mask_tos))
		return (0);
	/* match */
	return (1);
}

#ifdef INET6
static int
apply_filter6(fbmask, filt, pkt)
	u_int32_t	fbmask;
	struct flow_filter6 *filt;
	struct flowinfo_in6 *pkt;
{
	int i;

	if (filt->ff_flow6.fi6_family != AF_INET6)
		return (0);
	if ((fbmask & FIMB6_FLABEL) &&
	    filt->ff_flow6.fi6_flowlabel != pkt->fi6_flowlabel)
		return (0);
	if ((fbmask & FIMB6_PROTO) &&
	    filt->ff_flow6.fi6_proto != pkt->fi6_proto)
		return (0);
	if ((fbmask & FIMB6_SPORT) &&
	    filt->ff_flow6.fi6_sport != pkt->fi6_sport)
		return (0);
	if ((fbmask & FIMB6_DPORT) &&
	    filt->ff_flow6.fi6_dport != pkt->fi6_dport)
		return (0);
	if (fbmask & FIMB6_SADDR) {
		for (i = 0; i < 4; i++)
			if (filt->ff_flow6.fi6_src.s6_addr32[i] !=
			    (pkt->fi6_src.s6_addr32[i] &
			     filt->ff_mask6.mask6_src.s6_addr32[i]))
				return (0);
	}
	if (fbmask & FIMB6_DADDR) {
		for (i = 0; i < 4; i++)
			if (filt->ff_flow6.fi6_dst.s6_addr32[i] !=
			    (pkt->fi6_dst.s6_addr32[i] &
			     filt->ff_mask6.mask6_dst.s6_addr32[i]))
				return (0);
	}
	if ((fbmask & FIMB6_TCLASS) &&
	    filt->ff_flow6.fi6_tclass !=
	    (pkt->fi6_tclass & filt->ff_mask6.mask6_tclass))
		return (0);
	if ((fbmask & FIMB6_GPI) &&
	    filt->ff_flow6.fi6_gpi != pkt->fi6_gpi)
		return (0);
	/* match */
	return (1);
}
#endif /* INET6 */

/*
 *  filter handle:
 *	bit 20-28: index to the filter hash table
 *	bit  0-19: unique id in the hash bucket.
 */
static u_long
get_filt_handle(classifier, i)
	struct acc_classifier *classifier;
	int	i;
{
	static u_long handle_number = 1;
	u_long 	handle;
	struct acc_filter *afp;

	while (1) {
		handle = handle_number++ & 0x000fffff;

		if (LIST_EMPTY(&classifier->acc_filters[i]))
			break;

		LIST_FOREACH(afp, &classifier->acc_filters[i], f_chain)
			if ((afp->f_handle & 0x000fffff) == handle)
				break;
		if (afp == NULL)
			break;
		/* this handle is already used, try again */
	}

	return ((i << 20) | handle);
}

/* convert filter handle to filter pointer */
static struct acc_filter *
filth_to_filtp(classifier, handle)
	struct acc_classifier *classifier;
	u_long handle;
{
	struct acc_filter *afp;
	int	i;

	i = ACC_GET_HINDEX(handle);

	LIST_FOREACH(afp, &classifier->acc_filters[i], f_chain)
		if (afp->f_handle == handle)
			return (afp);

	return (NULL);
}

/* create flowinfo bitmask */
static u_int32_t
filt2fibmask(filt)
	struct flow_filter *filt;
{
	u_int32_t mask = 0;
#ifdef INET6
	struct flow_filter6 *filt6;
#endif

	switch (filt->ff_flow.fi_family) {
	case AF_INET:
		if (filt->ff_flow.fi_proto != 0)
			mask |= FIMB4_PROTO;
		if (filt->ff_flow.fi_tos != 0)
			mask |= FIMB4_TOS;
		if (filt->ff_flow.fi_dst.s_addr != 0)
			mask |= FIMB4_DADDR;
		if (filt->ff_flow.fi_src.s_addr != 0)
			mask |= FIMB4_SADDR;
		if (filt->ff_flow.fi_sport != 0)
			mask |= FIMB4_SPORT;
		if (filt->ff_flow.fi_dport != 0)
			mask |= FIMB4_DPORT;
		if (filt->ff_flow.fi_gpi != 0)
			mask |= FIMB4_GPI;
		break;
#ifdef INET6
	case AF_INET6:
		filt6 = (struct flow_filter6 *)filt;

		if (filt6->ff_flow6.fi6_proto != 0)
			mask |= FIMB6_PROTO;
		if (filt6->ff_flow6.fi6_tclass != 0)
			mask |= FIMB6_TCLASS;
		if (!IN6_IS_ADDR_UNSPECIFIED(&filt6->ff_flow6.fi6_dst))
			mask |= FIMB6_DADDR;
		if (!IN6_IS_ADDR_UNSPECIFIED(&filt6->ff_flow6.fi6_src))
			mask |= FIMB6_SADDR;
		if (filt6->ff_flow6.fi6_sport != 0)
			mask |= FIMB6_SPORT;
		if (filt6->ff_flow6.fi6_dport != 0)
			mask |= FIMB6_DPORT;
		if (filt6->ff_flow6.fi6_gpi != 0)
			mask |= FIMB6_GPI;
		if (filt6->ff_flow6.fi6_flowlabel != 0)
			mask |= FIMB6_FLABEL;
		break;
#endif /* INET6 */
	}
	return (mask);
}

/*
 * helper functions to handle IPv4 fragments.
 * currently only in-sequence fragments are handled.
 *	- fragment info is cached in a LRU list.
 *	- when a first fragment is found, cache its flow info.
 *	- when a non-first fragment is found, lookup the cache.
 */

struct ip4_frag {
    TAILQ_ENTRY(ip4_frag) ip4f_chain;
    char    ip4f_valid;
    u_short ip4f_id;
    struct flowinfo_in ip4f_info;
};

static TAILQ_HEAD(ip4f_list, ip4_frag) ip4f_list; /* IPv4 fragment cache */

#define	IP4F_TABSIZE		16	/* IPv4 fragment cache size */

static void
ip4f_cache(ip, fin)
	struct ip *ip;
	struct flowinfo_in *fin;
{
	struct ip4_frag *fp;

	if (TAILQ_EMPTY(&ip4f_list)) {
		/* first time call, allocate fragment cache entries. */
		if (ip4f_init() < 0)
			/* allocation failed! */
			return;
	}

	fp = ip4f_alloc();
	fp->ip4f_id = ip->ip_id;
	fp->ip4f_info.fi_proto = ip->ip_p;
	fp->ip4f_info.fi_src.s_addr = ip->ip_src.s_addr;
	fp->ip4f_info.fi_dst.s_addr = ip->ip_dst.s_addr;

	/* save port numbers */
	fp->ip4f_info.fi_sport = fin->fi_sport;
	fp->ip4f_info.fi_dport = fin->fi_dport;
	fp->ip4f_info.fi_gpi   = fin->fi_gpi;
}

static int
ip4f_lookup(ip, fin)
	struct ip *ip;
	struct flowinfo_in *fin;
{
	struct ip4_frag *fp;

	for (fp = TAILQ_FIRST(&ip4f_list); fp != NULL && fp->ip4f_valid;
	     fp = TAILQ_NEXT(fp, ip4f_chain))
		if (ip->ip_id == fp->ip4f_id &&
		    ip->ip_src.s_addr == fp->ip4f_info.fi_src.s_addr &&
		    ip->ip_dst.s_addr == fp->ip4f_info.fi_dst.s_addr &&
		    ip->ip_p == fp->ip4f_info.fi_proto) {
			/* found the matching entry */
			fin->fi_sport = fp->ip4f_info.fi_sport;
			fin->fi_dport = fp->ip4f_info.fi_dport;
			fin->fi_gpi   = fp->ip4f_info.fi_gpi;

			if ((ntohs(ip->ip_off) & IP_MF) == 0)
				/* this is the last fragment,
				   release the entry. */
				ip4f_free(fp);

			return (1);
		}

	/* no matching entry found */
	return (0);
}

static int
ip4f_init(void)
{
	struct ip4_frag *fp;
	int i;

	TAILQ_INIT(&ip4f_list);
	for (i=0; i<IP4F_TABSIZE; i++) {
		fp = malloc(sizeof(struct ip4_frag),
		       M_DEVBUF, M_NOWAIT);
		if (fp == NULL) {
			printf("ip4f_init: can't alloc %dth entry!\n", i);
			if (i == 0)
				return (-1);
			return (0);
		}
		fp->ip4f_valid = 0;
		TAILQ_INSERT_TAIL(&ip4f_list, fp, ip4f_chain);
	}
	return (0);
}

static struct ip4_frag *
ip4f_alloc(void)
{
	struct ip4_frag *fp;

	/* reclaim an entry at the tail, put it at the head */
	fp = TAILQ_LAST(&ip4f_list, ip4f_list);
	TAILQ_REMOVE(&ip4f_list, fp, ip4f_chain);
	fp->ip4f_valid = 1;
	TAILQ_INSERT_HEAD(&ip4f_list, fp, ip4f_chain);
	return (fp);
}

static void
ip4f_free(fp)
	struct ip4_frag *fp;
{
	TAILQ_REMOVE(&ip4f_list, fp, ip4f_chain);
	fp->ip4f_valid = 0;
	TAILQ_INSERT_TAIL(&ip4f_list, fp, ip4f_chain);
}

#endif /* ALTQ3_CLFIER_COMPAT */