xref: /freebsd/sys/netgraph/ng_car.c (revision 9b8db66402e94919ab47fc2d69863968e49e305a)

/*-
 * SPDX-License-Identifier: BSD-2-Clause
 *
 * Copyright (c) 2005 Nuno Antunes <nuno.antunes@gmail.com>
 * Copyright (c) 2007 Alexander Motin <mav@freebsd.org>
 * Copyright (c) 2019 Lutz Donnerhacke <lutz@donnerhacke.de>
 * 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 AUTHOR 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 AUTHOR 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.
 */

/*
 * ng_car - An implementation of committed access rate for netgraph
 *
 * TODO:
 *	- Sanitize input config values (impose some limits)
 *	- Implement DSCP marking for IPv4
 *	- Decouple functionality into a simple classifier (g/y/r)
 *	  and various action nodes (i.e. shape, dcsp, pcp)
 */

#include <sys/param.h>
#include <sys/errno.h>
#include <sys/kernel.h>
#include <sys/malloc.h>
#include <sys/mbuf.h>

#include <netgraph/ng_message.h>
#include <netgraph/ng_parse.h>
#include <netgraph/netgraph.h>
#include <netgraph/ng_car.h>

#include "qos.h"

#ifdef NG_SEPARATE_MALLOC
static MALLOC_DEFINE(M_NETGRAPH_CAR, "netgraph_car", "netgraph car node");
#else
#define M_NETGRAPH_CAR M_NETGRAPH
#endif

#define NG_CAR_QUEUE_SIZE	100	/* Maximum queue size for SHAPE mode */
#define NG_CAR_QUEUE_MIN_TH	8	/* Minimum RED threshold for SHAPE mode */

/* Hook private info */
struct hookinfo {
	hook_p		hook;		/* this (source) hook */
	hook_p		dest;		/* destination hook */

	int64_t 	tc;		/* committed token bucket counter */
	int64_t 	te;		/* exceeded/peak token bucket counter */
	struct bintime	lastRefill;	/* last token refill time */

	struct ng_car_hookconf conf;	/* hook configuration */
	struct ng_car_hookstats stats;	/* hook stats */

	struct mbuf	*q[NG_CAR_QUEUE_SIZE];	/* circular packet queue */
	u_int		q_first;	/* first queue element */
	u_int		q_last;		/* last queue element */
	struct callout	q_callout;	/* periodic queue processing routine */
	struct mtx	q_mtx;		/* queue mutex */
};

/* Private information for each node instance */
struct privdata {
	node_p node;				/* the node itself */
	struct hookinfo upper;			/* hook to upper layers */
	struct hookinfo lower;			/* hook to lower layers */
};
typedef struct privdata *priv_p;

static ng_constructor_t	ng_car_constructor;
static ng_rcvmsg_t	ng_car_rcvmsg;
static ng_shutdown_t	ng_car_shutdown;
static ng_newhook_t	ng_car_newhook;
static ng_rcvdata_t	ng_car_rcvdata;
static ng_disconnect_t	ng_car_disconnect;

static void	ng_car_refillhook(struct hookinfo *h);
static void	ng_car_schedule(struct hookinfo *h);
void		ng_car_q_event(node_p node, hook_p hook, void *arg, int arg2);
static void	ng_car_enqueue(struct hookinfo *h, item_p item);

/* Parse type for struct ng_car_hookstats */
static const struct ng_parse_struct_field ng_car_hookstats_type_fields[]
	= NG_CAR_HOOKSTATS;
static const struct ng_parse_type ng_car_hookstats_type = {
	&ng_parse_struct_type,
	&ng_car_hookstats_type_fields
};

/* Parse type for struct ng_car_bulkstats */
static const struct ng_parse_struct_field ng_car_bulkstats_type_fields[]
	= NG_CAR_BULKSTATS(&ng_car_hookstats_type);
static const struct ng_parse_type ng_car_bulkstats_type = {
	&ng_parse_struct_type,
	&ng_car_bulkstats_type_fields
};

/* Parse type for struct ng_car_hookconf */
static const struct ng_parse_struct_field ng_car_hookconf_type_fields[]
	= NG_CAR_HOOKCONF;
static const struct ng_parse_type ng_car_hookconf_type = {
	&ng_parse_struct_type,
	&ng_car_hookconf_type_fields
};

/* Parse type for struct ng_car_bulkconf */
static const struct ng_parse_struct_field ng_car_bulkconf_type_fields[]
	= NG_CAR_BULKCONF(&ng_car_hookconf_type);
static const struct ng_parse_type ng_car_bulkconf_type = {
	&ng_parse_struct_type,
	&ng_car_bulkconf_type_fields
};

/* Command list */
static struct ng_cmdlist ng_car_cmdlist[] = {
	{
	  NGM_CAR_COOKIE,
	  NGM_CAR_GET_STATS,
	  "getstats",
	  NULL,
	  &ng_car_bulkstats_type,
	},
	{
	  NGM_CAR_COOKIE,
	  NGM_CAR_CLR_STATS,
	  "clrstats",
	  NULL,
	  NULL,
	},
	{
	  NGM_CAR_COOKIE,
	  NGM_CAR_GETCLR_STATS,
	  "getclrstats",
	  NULL,
	  &ng_car_bulkstats_type,
	},

	{
	  NGM_CAR_COOKIE,
	  NGM_CAR_GET_CONF,
	  "getconf",
	  NULL,
	  &ng_car_bulkconf_type,
	},
	{
	  NGM_CAR_COOKIE,
	  NGM_CAR_SET_CONF,
	  "setconf",
	  &ng_car_bulkconf_type,
	  NULL,
	},
	{ 0 }
};

/* Netgraph node type descriptor */
static struct ng_type ng_car_typestruct = {
	.version =	NG_ABI_VERSION,
	.name =		NG_CAR_NODE_TYPE,
	.constructor =	ng_car_constructor,
	.rcvmsg =	ng_car_rcvmsg,
	.shutdown =	ng_car_shutdown,
	.newhook =	ng_car_newhook,
	.rcvdata =	ng_car_rcvdata,
	.disconnect =	ng_car_disconnect,
	.cmdlist =	ng_car_cmdlist,
};
NETGRAPH_INIT(car, &ng_car_typestruct);

/*
 * Node constructor
 */
static int
ng_car_constructor(node_p node)
{
	priv_p priv;

	/* Initialize private descriptor. */
	priv = malloc(sizeof(*priv), M_NETGRAPH_CAR, M_WAITOK | M_ZERO);

	NG_NODE_SET_PRIVATE(node, priv);
	priv->node = node;

	/*
	 * Arbitrary default values
	 */

	priv->upper.hook = NULL;
	priv->upper.dest = NULL;
	priv->upper.tc = priv->upper.conf.cbs = NG_CAR_CBS_MIN;
	priv->upper.te = priv->upper.conf.ebs = NG_CAR_EBS_MIN;
	priv->upper.conf.cir = NG_CAR_CIR_DFLT;
	priv->upper.conf.green_action = NG_CAR_ACTION_FORWARD;
	priv->upper.conf.yellow_action = NG_CAR_ACTION_FORWARD;
	priv->upper.conf.red_action = NG_CAR_ACTION_DROP;
	priv->upper.conf.mode = 0;
	getbinuptime(&priv->upper.lastRefill);
	priv->upper.q_first = 0;
	priv->upper.q_last = 0;
	ng_callout_init(&priv->upper.q_callout);
	mtx_init(&priv->upper.q_mtx, "ng_car_u", NULL, MTX_DEF);

	priv->lower.hook = NULL;
	priv->lower.dest = NULL;
	priv->lower.tc = priv->lower.conf.cbs = NG_CAR_CBS_MIN;
	priv->lower.te = priv->lower.conf.ebs = NG_CAR_EBS_MIN;
	priv->lower.conf.cir = NG_CAR_CIR_DFLT;
	priv->lower.conf.green_action = NG_CAR_ACTION_FORWARD;
	priv->lower.conf.yellow_action = NG_CAR_ACTION_FORWARD;
	priv->lower.conf.red_action = NG_CAR_ACTION_DROP;
	priv->lower.conf.mode = 0;
	priv->lower.lastRefill = priv->upper.lastRefill;
	priv->lower.q_first = 0;
	priv->lower.q_last = 0;
	ng_callout_init(&priv->lower.q_callout);
	mtx_init(&priv->lower.q_mtx, "ng_car_l", NULL, MTX_DEF);

	return (0);
}

/*
 * Add a hook.
 */
static int
ng_car_newhook(node_p node, hook_p hook, const char *name)
{
	const priv_p priv = NG_NODE_PRIVATE(node);

	if (strcmp(name, NG_CAR_HOOK_LOWER) == 0) {
		priv->lower.hook = hook;
		priv->upper.dest = hook;
		bzero(&priv->lower.stats, sizeof(priv->lower.stats));
		NG_HOOK_SET_PRIVATE(hook, &priv->lower);
	} else if (strcmp(name, NG_CAR_HOOK_UPPER) == 0) {
		priv->upper.hook = hook;
		priv->lower.dest = hook;
		bzero(&priv->upper.stats, sizeof(priv->upper.stats));
		NG_HOOK_SET_PRIVATE(hook, &priv->upper);
	} else
		return (EINVAL);
	return(0);
}

/*
 * Data has arrived.
 */
static int
ng_car_rcvdata(hook_p hook, item_p item )
{
	struct hookinfo *const hinfo = NG_HOOK_PRIVATE(hook);
	struct mbuf *m;
	struct m_qos_color *colp;
	enum qos_color col;
	int error = 0;
	u_int len;

	/* If queue is not empty now then enqueue packet. */
	if (hinfo->q_first != hinfo->q_last) {
		ng_car_enqueue(hinfo, item);
		return (0);
	}

	m = NGI_M(item);

#define NG_CAR_PERFORM_MATCH_ACTION(a,col)			\
	do {						\
		switch (a) {				\
		case NG_CAR_ACTION_FORWARD:		\
			/* Do nothing. */		\
			break;				\
		case NG_CAR_ACTION_MARK:		\
			if (colp == NULL) {		\
				colp = (void *)m_tag_alloc(		\
				    M_QOS_COOKIE, M_QOS_COLOR,		\
				    MTAG_SIZE(m_qos_color), M_NOWAIT);	\
				if (colp != NULL)			\
				    m_tag_prepend(m, &colp->tag);	\
			}				\
			if (colp != NULL)		\
			    colp->color = col;		\
			break;				\
		case NG_CAR_ACTION_DROP:		\
		default:				\
			/* Drop packet and return. */	\
			NG_FREE_ITEM(item);		\
			++hinfo->stats.dropped_pkts;	\
			return (0);			\
		}					\
	} while (0)

	/* Packet is counted as 128 tokens for better resolution */
	if (hinfo->conf.opt & NG_CAR_COUNT_PACKETS) {
		len = 128;
	} else {
		len = m->m_pkthdr.len;
	}

	/* Determine current color of the packet (default green) */
	colp = (void *)m_tag_locate(m, M_QOS_COOKIE, M_QOS_COLOR, NULL);
	if ((hinfo->conf.opt & NG_CAR_COLOR_AWARE) && (colp != NULL))
	    col = colp->color;
	else
	    col = QOS_COLOR_GREEN;

	/* Check committed token bucket. */
	if (hinfo->tc - len >= 0 && col <= QOS_COLOR_GREEN) {
		/* This packet is green. */
		++hinfo->stats.green_pkts;
		hinfo->tc -= len;
		NG_CAR_PERFORM_MATCH_ACTION(
		    hinfo->conf.green_action,
		    QOS_COLOR_GREEN);
	} else {
		/* Refill only if not green without it. */
		ng_car_refillhook(hinfo);

		 /* Check committed token bucket again after refill. */
		if (hinfo->tc - len >= 0 && col <= QOS_COLOR_GREEN) {
			/* This packet is green */
			++hinfo->stats.green_pkts;
			hinfo->tc -= len;
			NG_CAR_PERFORM_MATCH_ACTION(
			    hinfo->conf.green_action,
			    QOS_COLOR_GREEN);

		/* If not green and mode is SHAPE, enqueue packet. */
		} else if (hinfo->conf.mode == NG_CAR_SHAPE) {
			ng_car_enqueue(hinfo, item);
			return (0);

		/* If not green and mode is RED, calculate probability. */
		} else if (hinfo->conf.mode == NG_CAR_RED) {
			/* Is packet is bigger then extended burst? */
			if (len - (hinfo->tc - len) > hinfo->conf.ebs ||
			    col >= QOS_COLOR_RED) {
				/* This packet is definitely red. */
				++hinfo->stats.red_pkts;
				hinfo->te = 0;
				NG_CAR_PERFORM_MATCH_ACTION(
					hinfo->conf.red_action,
					QOS_COLOR_RED);

			/* Use token bucket to simulate RED-like drop
			   probability. */
			} else if (hinfo->te + (len - hinfo->tc) < hinfo->conf.ebs &&
				   col <= QOS_COLOR_YELLOW) {
				/* This packet is yellow */
				++hinfo->stats.yellow_pkts;
				hinfo->te += len - hinfo->tc;
				/* Go to negative tokens. */
				hinfo->tc -= len;
				NG_CAR_PERFORM_MATCH_ACTION(
				    hinfo->conf.yellow_action,
				    QOS_COLOR_YELLOW);
			} else {
				/* This packet is probably red. */
				++hinfo->stats.red_pkts;
				hinfo->te = 0;
				NG_CAR_PERFORM_MATCH_ACTION(
				    hinfo->conf.red_action,
				    QOS_COLOR_RED);
			}
		/* If not green and mode is SINGLE/DOUBLE RATE. */
		} else {
			/* Check extended token bucket. */
			if (hinfo->te - len >= 0 && col <= QOS_COLOR_YELLOW) {
				/* This packet is yellow */
				++hinfo->stats.yellow_pkts;
				hinfo->te -= len;
				NG_CAR_PERFORM_MATCH_ACTION(
				    hinfo->conf.yellow_action,
				    QOS_COLOR_YELLOW);
			} else {
				/* This packet is red */
				++hinfo->stats.red_pkts;
				NG_CAR_PERFORM_MATCH_ACTION(
				    hinfo->conf.red_action,
				    QOS_COLOR_RED);
			}
		}
	}

#undef NG_CAR_PERFORM_MATCH_ACTION

	NG_FWD_ITEM_HOOK(error, item, hinfo->dest);
	if (error != 0)
		++hinfo->stats.errors;
	++hinfo->stats.passed_pkts;

	return (error);
}

/*
 * Receive a control message.
 */
static int
ng_car_rcvmsg(node_p node, item_p item, hook_p lasthook)
{
	const priv_p priv = NG_NODE_PRIVATE(node);
	struct ng_mesg *resp = NULL;
	int error = 0;
	struct ng_mesg *msg;

	NGI_GET_MSG(item, msg);
	switch (msg->header.typecookie) {
	case NGM_CAR_COOKIE:
		switch (msg->header.cmd) {
		case NGM_CAR_GET_STATS:
		case NGM_CAR_GETCLR_STATS:
			{
				struct ng_car_bulkstats *bstats;

				NG_MKRESPONSE(resp, msg,
					sizeof(*bstats), M_NOWAIT);
				if (resp == NULL) {
					error = ENOMEM;
					break;
				}
				bstats = (struct ng_car_bulkstats *)resp->data;

				bcopy(&priv->upper.stats, &bstats->downstream,
				    sizeof(bstats->downstream));
				bcopy(&priv->lower.stats, &bstats->upstream,
				    sizeof(bstats->upstream));
			}
			if (msg->header.cmd == NGM_CAR_GET_STATS)
				break;
		case NGM_CAR_CLR_STATS:
			bzero(&priv->upper.stats,
				sizeof(priv->upper.stats));
			bzero(&priv->lower.stats,
				sizeof(priv->lower.stats));
			break;
		case NGM_CAR_GET_CONF:
			{
				struct ng_car_bulkconf *bconf;

				NG_MKRESPONSE(resp, msg,
					sizeof(*bconf), M_NOWAIT);
				if (resp == NULL) {
					error = ENOMEM;
					break;
				}
				bconf = (struct ng_car_bulkconf *)resp->data;

				bcopy(&priv->upper.conf, &bconf->downstream,
				    sizeof(bconf->downstream));
				bcopy(&priv->lower.conf, &bconf->upstream,
				    sizeof(bconf->upstream));
				/* Convert internal 1/(8*128) of pps into pps */
				if (bconf->downstream.opt & NG_CAR_COUNT_PACKETS) {
				    bconf->downstream.cir /= 1024;
				    bconf->downstream.pir /= 1024;
				    bconf->downstream.cbs /= 128;
				    bconf->downstream.ebs /= 128;
				}
				if (bconf->upstream.opt & NG_CAR_COUNT_PACKETS) {
				    bconf->upstream.cir /= 1024;
				    bconf->upstream.pir /= 1024;
				    bconf->upstream.cbs /= 128;
				    bconf->upstream.ebs /= 128;
				}
			}
			break;
		case NGM_CAR_SET_CONF:
			{
				struct ng_car_bulkconf *const bconf =
				(struct ng_car_bulkconf *)msg->data;

				/* Check for invalid or illegal config. */
				if (msg->header.arglen != sizeof(*bconf)) {
					error = EINVAL;
					break;
				}
				/* Convert pps into internal 1/(8*128) of pps */
				if (bconf->downstream.opt & NG_CAR_COUNT_PACKETS) {
				    bconf->downstream.cir *= 1024;
				    bconf->downstream.pir *= 1024;
				    bconf->downstream.cbs *= 128;
				    bconf->downstream.ebs *= 128;
				}
				if (bconf->upstream.opt & NG_CAR_COUNT_PACKETS) {
				    bconf->upstream.cir *= 1024;
				    bconf->upstream.pir *= 1024;
				    bconf->upstream.cbs *= 128;
				    bconf->upstream.ebs *= 128;
				}
				if ((bconf->downstream.cir > 1000000000) ||
				    (bconf->downstream.pir > 1000000000) ||
				    (bconf->upstream.cir > 1000000000) ||
				    (bconf->upstream.pir > 1000000000) ||
				    (bconf->downstream.cbs == 0 &&
					bconf->downstream.ebs == 0) ||
				    (bconf->upstream.cbs == 0 &&
					bconf->upstream.ebs == 0))
				{
					error = EINVAL;
					break;
				}
				if ((bconf->upstream.mode == NG_CAR_SHAPE) &&
				    (bconf->upstream.cir == 0)) {
					error = EINVAL;
					break;
				}
				if ((bconf->downstream.mode == NG_CAR_SHAPE) &&
				    (bconf->downstream.cir == 0)) {
					error = EINVAL;
					break;
				}

				/* Copy downstream config. */
				bcopy(&bconf->downstream, &priv->upper.conf,
				    sizeof(priv->upper.conf));
    				priv->upper.tc = priv->upper.conf.cbs;
				if (priv->upper.conf.mode == NG_CAR_RED ||
				    priv->upper.conf.mode == NG_CAR_SHAPE) {
					priv->upper.te = 0;
				} else {
					priv->upper.te = priv->upper.conf.ebs;
				}

				/* Copy upstream config. */
				bcopy(&bconf->upstream, &priv->lower.conf,
				    sizeof(priv->lower.conf));
    				priv->lower.tc = priv->lower.conf.cbs;
				if (priv->lower.conf.mode == NG_CAR_RED ||
				    priv->lower.conf.mode == NG_CAR_SHAPE) {
					priv->lower.te = 0;
				} else {
					priv->lower.te = priv->lower.conf.ebs;
				}
			}
			break;
		default:
			error = EINVAL;
			break;
		}
		break;
	default:
		error = EINVAL;
		break;
	}
	NG_RESPOND_MSG(error, node, item, resp);
	NG_FREE_MSG(msg);
	return (error);
}

/*
 * Do local shutdown processing.
 */
static int
ng_car_shutdown(node_p node)
{
	const priv_p priv = NG_NODE_PRIVATE(node);

	ng_uncallout(&priv->upper.q_callout, node);
	ng_uncallout(&priv->lower.q_callout, node);
	mtx_destroy(&priv->upper.q_mtx);
	mtx_destroy(&priv->lower.q_mtx);
	NG_NODE_UNREF(priv->node);
	free(priv, M_NETGRAPH_CAR);
	return (0);
}

/*
 * Hook disconnection.
 *
 * For this type, removal of the last link destroys the node.
 */
static int
ng_car_disconnect(hook_p hook)
{
	struct hookinfo *const hinfo = NG_HOOK_PRIVATE(hook);
	const node_p node = NG_HOOK_NODE(hook);
	const priv_p priv = NG_NODE_PRIVATE(node);

	if (hinfo) {
		/* Purge queue if not empty. */
		while (hinfo->q_first != hinfo->q_last) {
			NG_FREE_M(hinfo->q[hinfo->q_first]);
			hinfo->q_first++;
			if (hinfo->q_first >= NG_CAR_QUEUE_SIZE)
		    		hinfo->q_first = 0;
		}
		/* Remove hook refs. */
		if (hinfo->hook == priv->upper.hook)
			priv->lower.dest = NULL;
		else
			priv->upper.dest = NULL;
		hinfo->hook = NULL;
	}
	/* Already shutting down? */
	if ((NG_NODE_NUMHOOKS(NG_HOOK_NODE(hook)) == 0)
	    && (NG_NODE_IS_VALID(NG_HOOK_NODE(hook))))
		ng_rmnode_self(NG_HOOK_NODE(hook));
	return (0);
}

/*
 * Hook's token buckets refillment.
 */
static void
ng_car_refillhook(struct hookinfo *h)
{
	struct bintime newt, deltat;
	unsigned int deltat_us;

	/* Get current time. */
	getbinuptime(&newt);

	/* Get time delta since last refill. */
	deltat = newt;
	bintime_sub(&deltat, &h->lastRefill);

	/* Time must go forward. */
	if (deltat.sec < 0) {
	    h->lastRefill = newt;
	    return;
	}

	/* But not too far forward. */
	if (deltat.sec >= 1000) {
	    deltat_us = (1000 << 20);
	} else {
	    /* convert bintime to the 1/(2^20) of sec */
	    deltat_us = (deltat.sec << 20) + (deltat.frac >> 44);
	}

	if (h->conf.mode == NG_CAR_SINGLE_RATE) {
		int64_t	delta;
		/* Refill committed token bucket. */
		h->tc += (h->conf.cir * deltat_us) >> 23;
		delta = h->tc - h->conf.cbs;
		if (delta > 0) {
			h->tc = h->conf.cbs;

			/* Refill exceeded token bucket. */
			h->te += delta;
			if (h->te > ((int64_t)h->conf.ebs))
				h->te = h->conf.ebs;
		}

	} else if (h->conf.mode == NG_CAR_DOUBLE_RATE) {
		/* Refill committed token bucket. */
		h->tc += (h->conf.cir * deltat_us) >> 23;
		if (h->tc > ((int64_t)h->conf.cbs))
			h->tc = h->conf.cbs;

		/* Refill peak token bucket. */
		h->te += (h->conf.pir * deltat_us) >> 23;
		if (h->te > ((int64_t)h->conf.ebs))
			h->te = h->conf.ebs;

	} else { /* RED or SHAPE mode. */
		/* Refill committed token bucket. */
		h->tc += (h->conf.cir * deltat_us) >> 23;
		if (h->tc > ((int64_t)h->conf.cbs))
			h->tc = h->conf.cbs;
	}

	/* Remember this moment. */
	h->lastRefill = newt;
}

/*
 * Schedule callout when we will have required tokens.
 */
static void
ng_car_schedule(struct hookinfo *hinfo)
{
	int 	delay;

	delay = (-(hinfo->tc)) * hz * 8 / hinfo->conf.cir + 1;

	ng_callout(&hinfo->q_callout, NG_HOOK_NODE(hinfo->hook), hinfo->hook,
	    delay, &ng_car_q_event, NULL, 0);
}

/*
 * Queue processing callout handler.
 */
void
ng_car_q_event(node_p node, hook_p hook, void *arg, int arg2)
{
	struct hookinfo	*hinfo = NG_HOOK_PRIVATE(hook);
	struct mbuf 	*m;
	int		error;

	/* Refill tokens for time we have slept. */
	ng_car_refillhook(hinfo);

	/* If we have some tokens */
	while (hinfo->tc >= 0) {
		/* Send packet. */
		m = hinfo->q[hinfo->q_first];
		NG_SEND_DATA_ONLY(error, hinfo->dest, m);
		if (error != 0)
			++hinfo->stats.errors;
		++hinfo->stats.passed_pkts;

		/* Get next one. */
		hinfo->q_first++;
		if (hinfo->q_first >= NG_CAR_QUEUE_SIZE)
			hinfo->q_first = 0;

		/* Stop if none left. */
		if (hinfo->q_first == hinfo->q_last)
			break;

		/* If we have more packet, try it. */
		m = hinfo->q[hinfo->q_first];
		if (hinfo->conf.opt & NG_CAR_COUNT_PACKETS) {
			hinfo->tc -= 128;
		} else {
			hinfo->tc -= m->m_pkthdr.len;
		}
	}

	/* If something left */
	if (hinfo->q_first != hinfo->q_last)
		/* Schedule queue processing. */
		ng_car_schedule(hinfo);
}

/*
 * Enqueue packet.
 */
static void
ng_car_enqueue(struct hookinfo *hinfo, item_p item)
{
	struct mbuf 	   *m;
	int		   len;
	struct m_qos_color *colp;
	enum qos_color	   col;

	NGI_GET_M(item, m);
	NG_FREE_ITEM(item);

	/* Determine current color of the packet (default green) */
	colp = (void *)m_tag_locate(m, M_QOS_COOKIE, M_QOS_COLOR, NULL);
	if ((hinfo->conf.opt & NG_CAR_COLOR_AWARE) && (colp != NULL))
	    col = colp->color;
	else
	    col = QOS_COLOR_GREEN;

	/* Lock queue mutex. */
	mtx_lock(&hinfo->q_mtx);

	/* Calculate used queue length. */
	len = hinfo->q_last - hinfo->q_first;
	if (len < 0)
		len += NG_CAR_QUEUE_SIZE;

	/* If queue is overflowed or we have no RED tokens. */
	if ((len >= (NG_CAR_QUEUE_SIZE - 1)) ||
	    (hinfo->te + len >= NG_CAR_QUEUE_SIZE) ||
	    (col >= QOS_COLOR_RED)) {
		/* Drop packet. */
		++hinfo->stats.red_pkts;
		++hinfo->stats.dropped_pkts;
		NG_FREE_M(m);

		hinfo->te = 0;
	} else {
		/* This packet is yellow. */
		++hinfo->stats.yellow_pkts;

		/* Enqueue packet. */
		hinfo->q[hinfo->q_last] = m;
		hinfo->q_last++;
		if (hinfo->q_last >= NG_CAR_QUEUE_SIZE)
			hinfo->q_last = 0;

		/* Use RED tokens. */
		if (len > NG_CAR_QUEUE_MIN_TH)
			hinfo->te += len - NG_CAR_QUEUE_MIN_TH;

		/* If this is a first packet in the queue. */
		if (len == 0) {
			if (hinfo->conf.opt & NG_CAR_COUNT_PACKETS) {
				hinfo->tc -= 128;
			} else {
				hinfo->tc -= m->m_pkthdr.len;
			}

			/* Schedule queue processing. */
			ng_car_schedule(hinfo);
		}
	}

	/* Unlock queue mutex. */
	mtx_unlock(&hinfo->q_mtx);
}