[mirror_ubuntu-zesty-kernel.git] / net / sched / sch_netem.c

/*
 * net/sched/sch_netem.c	Network emulator
 *
 * 		This program is free software; you can redistribute it and/or
 * 		modify it under the terms of the GNU General Public License
 * 		as published by the Free Software Foundation; either version
 * 		2 of the License.
 *
 *  		Many of the algorithms and ideas for this came from
 *		NIST Net which is not copyrighted.
 *
 * Authors:	Stephen Hemminger <shemminger@osdl.org>
 *		Catalin(ux aka Dino) BOIE <catab at umbrella dot ro>
 */

#include <linux/module.h>
#include <linux/slab.h>
#include <linux/types.h>
#include <linux/kernel.h>
#include <linux/errno.h>
#include <linux/skbuff.h>
#include <linux/rtnetlink.h>

#include <net/netlink.h>
#include <net/pkt_sched.h>

#define VERSION "1.2"

/*	Network Emulation Queuing algorithm.
	====================================

	Sources: [1] Mark Carson, Darrin Santay, "NIST Net - A Linux-based
		 Network Emulation Tool
		 [2] Luigi Rizzo, DummyNet for FreeBSD

	 ----------------------------------------------------------------

	 This started out as a simple way to delay outgoing packets to
	 test TCP but has grown to include most of the functionality
	 of a full blown network emulator like NISTnet. It can delay
	 packets and add random jitter (and correlation). The random
	 distribution can be loaded from a table as well to provide
	 normal, Pareto, or experimental curves. Packet loss,
	 duplication, and reordering can also be emulated.

	 This qdisc does not do classification that can be handled in
	 layering other disciplines.  It does not need to do bandwidth
	 control either since that can be handled by using token
	 bucket or other rate control.
*/

struct netem_sched_data {
	struct Qdisc	*qdisc;
	struct qdisc_watchdog watchdog;

	psched_tdiff_t latency;
	psched_tdiff_t jitter;

	u32 loss;
	u32 limit;
	u32 counter;
	u32 gap;
	u32 duplicate;
	u32 reorder;
	u32 corrupt;

	struct crndstate {
		u32 last;
		u32 rho;
	} delay_cor, loss_cor, dup_cor, reorder_cor, corrupt_cor;

	struct disttable {
		u32  size;
		s16 table[0];
	} *delay_dist;
};

/* Time stamp put into socket buffer control block */
struct netem_skb_cb {
	psched_time_t	time_to_send;
};

static inline struct netem_skb_cb *netem_skb_cb(struct sk_buff *skb)
{
	BUILD_BUG_ON(sizeof(skb->cb) <
		sizeof(struct qdisc_skb_cb) + sizeof(struct netem_skb_cb));
	return (struct netem_skb_cb *)qdisc_skb_cb(skb)->data;
}

/* init_crandom - initialize correlated random number generator
 * Use entropy source for initial seed.
 */
static void init_crandom(struct crndstate *state, unsigned long rho)
{
	state->rho = rho;
	state->last = net_random();
}

/* get_crandom - correlated random number generator
 * Next number depends on last value.
 * rho is scaled to avoid floating point.
 */
static u32 get_crandom(struct crndstate *state)
{
	u64 value, rho;
	unsigned long answer;

	if (state->rho == 0)	/* no correlation */
		return net_random();

	value = net_random();
	rho = (u64)state->rho + 1;
	answer = (value * ((1ull<<32) - rho) + state->last * rho) >> 32;
	state->last = answer;
	return answer;
}

/* tabledist - return a pseudo-randomly distributed value with mean mu and
 * std deviation sigma.  Uses table lookup to approximate the desired
 * distribution, and a uniformly-distributed pseudo-random source.
 */
static psched_tdiff_t tabledist(psched_tdiff_t mu, psched_tdiff_t sigma,
				struct crndstate *state,
				const struct disttable *dist)
{
	psched_tdiff_t x;
	long t;
	u32 rnd;

	if (sigma == 0)
		return mu;

	rnd = get_crandom(state);

	/* default uniform distribution */
	if (dist == NULL)
		return (rnd % (2*sigma)) - sigma + mu;

	t = dist->table[rnd % dist->size];
	x = (sigma % NETEM_DIST_SCALE) * t;
	if (x >= 0)
		x += NETEM_DIST_SCALE/2;
	else
		x -= NETEM_DIST_SCALE/2;

	return  x / NETEM_DIST_SCALE + (sigma / NETEM_DIST_SCALE) * t + mu;
}

/*
 * Insert one skb into qdisc.
 * Note: parent depends on return value to account for queue length.
 * 	NET_XMIT_DROP: queue length didn't change.
 *      NET_XMIT_SUCCESS: one skb was queued.
 */
static int netem_enqueue(struct sk_buff *skb, struct Qdisc *sch)
{
	struct netem_sched_data *q = qdisc_priv(sch);
	/* We don't fill cb now as skb_unshare() may invalidate it */
	struct netem_skb_cb *cb;
	struct sk_buff *skb2;
	int ret;
	int count = 1;

	pr_debug("netem_enqueue skb=%p\n", skb);

	/* Random duplication */
	if (q->duplicate && q->duplicate >= get_crandom(&q->dup_cor))
		++count;

	/* Random packet drop 0 => none, ~0 => all */
	if (q->loss && q->loss >= get_crandom(&q->loss_cor))
		--count;

	if (count == 0) {
		sch->qstats.drops++;
		kfree_skb(skb);
		return NET_XMIT_SUCCESS | __NET_XMIT_BYPASS;
	}

	skb_orphan(skb);

	/*
	 * If we need to duplicate packet, then re-insert at top of the
	 * qdisc tree, since parent queuer expects that only one
	 * skb will be queued.
	 */
	if (count > 1 && (skb2 = skb_clone(skb, GFP_ATOMIC)) != NULL) {
		struct Qdisc *rootq = qdisc_root(sch);
		u32 dupsave = q->duplicate; /* prevent duplicating a dup... */
		q->duplicate = 0;

		qdisc_enqueue_root(skb2, rootq);
		q->duplicate = dupsave;
	}

	/*
	 * Randomized packet corruption.
	 * Make copy if needed since we are modifying
	 * If packet is going to be hardware checksummed, then
	 * do it now in software before we mangle it.
	 */
	if (q->corrupt && q->corrupt >= get_crandom(&q->corrupt_cor)) {
		if (!(skb = skb_unshare(skb, GFP_ATOMIC)) ||
		    (skb->ip_summed == CHECKSUM_PARTIAL &&
		     skb_checksum_help(skb))) {
			sch->qstats.drops++;
			return NET_XMIT_DROP;
		}

		skb->data[net_random() % skb_headlen(skb)] ^= 1<<(net_random() % 8);
	}

	cb = netem_skb_cb(skb);
	if (q->gap == 0 || 		/* not doing reordering */
	    q->counter < q->gap || 	/* inside last reordering gap */
	    q->reorder < get_crandom(&q->reorder_cor)) {
		psched_time_t now;
		psched_tdiff_t delay;

		delay = tabledist(q->latency, q->jitter,
				  &q->delay_cor, q->delay_dist);

		now = psched_get_time();
		cb->time_to_send = now + delay;
		++q->counter;
		ret = qdisc_enqueue(skb, q->qdisc);
	} else {
		/*
		 * Do re-ordering by putting one out of N packets at the front
		 * of the queue.
		 */
		cb->time_to_send = psched_get_time();
		q->counter = 0;

		__skb_queue_head(&q->qdisc->q, skb);
		q->qdisc->qstats.backlog += qdisc_pkt_len(skb);
		q->qdisc->qstats.requeues++;
		ret = NET_XMIT_SUCCESS;
	}

	if (likely(ret == NET_XMIT_SUCCESS)) {
		sch->q.qlen++;
		qdisc_bstats_update(sch, skb);
	} else if (net_xmit_drop_count(ret)) {
		sch->qstats.drops++;
	}

	pr_debug("netem: enqueue ret %d\n", ret);
	return ret;
}

static unsigned int netem_drop(struct Qdisc* sch)
{
	struct netem_sched_data *q = qdisc_priv(sch);
	unsigned int len = 0;

	if (q->qdisc->ops->drop && (len = q->qdisc->ops->drop(q->qdisc)) != 0) {
		sch->q.qlen--;
		sch->qstats.drops++;
	}
	return len;
}

static struct sk_buff *netem_dequeue(struct Qdisc *sch)
{
	struct netem_sched_data *q = qdisc_priv(sch);
	struct sk_buff *skb;

	if (sch->flags & TCQ_F_THROTTLED)
		return NULL;

	skb = q->qdisc->ops->peek(q->qdisc);
	if (skb) {
		const struct netem_skb_cb *cb = netem_skb_cb(skb);
		psched_time_t now = psched_get_time();

		/* if more time remaining? */
		if (cb->time_to_send <= now) {
			skb = qdisc_dequeue_peeked(q->qdisc);
			if (unlikely(!skb))
				return NULL;

#ifdef CONFIG_NET_CLS_ACT
			/*
			 * If it's at ingress let's pretend the delay is
			 * from the network (tstamp will be updated).
			 */
			if (G_TC_FROM(skb->tc_verd) & AT_INGRESS)
				skb->tstamp.tv64 = 0;
#endif
			pr_debug("netem_dequeue: return skb=%p\n", skb);
			sch->q.qlen--;
			return skb;
		}

		qdisc_watchdog_schedule(&q->watchdog, cb->time_to_send);
	}

	return NULL;
}

static void netem_reset(struct Qdisc *sch)
{
	struct netem_sched_data *q = qdisc_priv(sch);

	qdisc_reset(q->qdisc);
	sch->q.qlen = 0;
	qdisc_watchdog_cancel(&q->watchdog);
}

/*
 * Distribution data is a variable size payload containing
 * signed 16 bit values.
 */
static int get_dist_table(struct Qdisc *sch, const struct nlattr *attr)
{
	struct netem_sched_data *q = qdisc_priv(sch);
	unsigned long n = nla_len(attr)/sizeof(__s16);
	const __s16 *data = nla_data(attr);
	spinlock_t *root_lock;
	struct disttable *d;
	int i;

	if (n > 65536)
		return -EINVAL;

	d = kmalloc(sizeof(*d) + n*sizeof(d->table[0]), GFP_KERNEL);
	if (!d)
		return -ENOMEM;

	d->size = n;
	for (i = 0; i < n; i++)
		d->table[i] = data[i];

	root_lock = qdisc_root_sleeping_lock(sch);

	spin_lock_bh(root_lock);
	kfree(q->delay_dist);
	q->delay_dist = d;
	spin_unlock_bh(root_lock);
	return 0;
}

static void get_correlation(struct Qdisc *sch, const struct nlattr *attr)
{
	struct netem_sched_data *q = qdisc_priv(sch);
	const struct tc_netem_corr *c = nla_data(attr);

	init_crandom(&q->delay_cor, c->delay_corr);
	init_crandom(&q->loss_cor, c->loss_corr);
	init_crandom(&q->dup_cor, c->dup_corr);
}

static void get_reorder(struct Qdisc *sch, const struct nlattr *attr)
{
	struct netem_sched_data *q = qdisc_priv(sch);
	const struct tc_netem_reorder *r = nla_data(attr);

	q->reorder = r->probability;
	init_crandom(&q->reorder_cor, r->correlation);
}

static void get_corrupt(struct Qdisc *sch, const struct nlattr *attr)
{
	struct netem_sched_data *q = qdisc_priv(sch);
	const struct tc_netem_corrupt *r = nla_data(attr);

	q->corrupt = r->probability;
	init_crandom(&q->corrupt_cor, r->correlation);
}

static const struct nla_policy netem_policy[TCA_NETEM_MAX + 1] = {
	[TCA_NETEM_CORR]	= { .len = sizeof(struct tc_netem_corr) },
	[TCA_NETEM_REORDER]	= { .len = sizeof(struct tc_netem_reorder) },
	[TCA_NETEM_CORRUPT]	= { .len = sizeof(struct tc_netem_corrupt) },
};

static int parse_attr(struct nlattr *tb[], int maxtype, struct nlattr *nla,
		      const struct nla_policy *policy, int len)
{
	int nested_len = nla_len(nla) - NLA_ALIGN(len);

	if (nested_len < 0)
		return -EINVAL;
	if (nested_len >= nla_attr_size(0))
		return nla_parse(tb, maxtype, nla_data(nla) + NLA_ALIGN(len),
				 nested_len, policy);
	memset(tb, 0, sizeof(struct nlattr *) * (maxtype + 1));
	return 0;
}

/* Parse netlink message to set options */
static int netem_change(struct Qdisc *sch, struct nlattr *opt)
{
	struct netem_sched_data *q = qdisc_priv(sch);
	struct nlattr *tb[TCA_NETEM_MAX + 1];
	struct tc_netem_qopt *qopt;
	int ret;

	if (opt == NULL)
		return -EINVAL;

	qopt = nla_data(opt);
	ret = parse_attr(tb, TCA_NETEM_MAX, opt, netem_policy, sizeof(*qopt));
	if (ret < 0)
		return ret;

	ret = fifo_set_limit(q->qdisc, qopt->limit);
	if (ret) {
		pr_debug("netem: can't set fifo limit\n");
		return ret;
	}

	q->latency = qopt->latency;
	q->jitter = qopt->jitter;
	q->limit = qopt->limit;
	q->gap = qopt->gap;
	q->counter = 0;
	q->loss = qopt->loss;
	q->duplicate = qopt->duplicate;

	/* for compatibility with earlier versions.
	 * if gap is set, need to assume 100% probability
	 */
	if (q->gap)
		q->reorder = ~0;

	if (tb[TCA_NETEM_CORR])
		get_correlation(sch, tb[TCA_NETEM_CORR]);

	if (tb[TCA_NETEM_DELAY_DIST]) {
		ret = get_dist_table(sch, tb[TCA_NETEM_DELAY_DIST]);
		if (ret)
			return ret;
	}

	if (tb[TCA_NETEM_REORDER])
		get_reorder(sch, tb[TCA_NETEM_REORDER]);

	if (tb[TCA_NETEM_CORRUPT])
		get_corrupt(sch, tb[TCA_NETEM_CORRUPT]);

	return 0;
}

/*
 * Special case version of FIFO queue for use by netem.
 * It queues in order based on timestamps in skb's
 */
struct fifo_sched_data {
	u32 limit;
	psched_time_t oldest;
};

static int tfifo_enqueue(struct sk_buff *nskb, struct Qdisc *sch)
{
	struct fifo_sched_data *q = qdisc_priv(sch);
	struct sk_buff_head *list = &sch->q;
	psched_time_t tnext = netem_skb_cb(nskb)->time_to_send;
	struct sk_buff *skb;

	if (likely(skb_queue_len(list) < q->limit)) {
		/* Optimize for add at tail */
		if (likely(skb_queue_empty(list) || tnext >= q->oldest)) {
			q->oldest = tnext;
			return qdisc_enqueue_tail(nskb, sch);
		}

		skb_queue_reverse_walk(list, skb) {
			const struct netem_skb_cb *cb = netem_skb_cb(skb);

			if (tnext >= cb->time_to_send)
				break;
		}

		__skb_queue_after(list, skb, nskb);

		sch->qstats.backlog += qdisc_pkt_len(nskb);
		qdisc_bstats_update(sch, nskb);

		return NET_XMIT_SUCCESS;
	}

	return qdisc_reshape_fail(nskb, sch);
}

static int tfifo_init(struct Qdisc *sch, struct nlattr *opt)
{
	struct fifo_sched_data *q = qdisc_priv(sch);

	if (opt) {
		struct tc_fifo_qopt *ctl = nla_data(opt);
		if (nla_len(opt) < sizeof(*ctl))
			return -EINVAL;

		q->limit = ctl->limit;
	} else
		q->limit = max_t(u32, qdisc_dev(sch)->tx_queue_len, 1);

	q->oldest = PSCHED_PASTPERFECT;
	return 0;
}

static int tfifo_dump(struct Qdisc *sch, struct sk_buff *skb)
{
	struct fifo_sched_data *q = qdisc_priv(sch);
	struct tc_fifo_qopt opt = { .limit = q->limit };

	NLA_PUT(skb, TCA_OPTIONS, sizeof(opt), &opt);
	return skb->len;

nla_put_failure:
	return -1;
}

static struct Qdisc_ops tfifo_qdisc_ops __read_mostly = {
	.id		=	"tfifo",
	.priv_size	=	sizeof(struct fifo_sched_data),
	.enqueue	=	tfifo_enqueue,
	.dequeue	=	qdisc_dequeue_head,
	.peek		=	qdisc_peek_head,
	.drop		=	qdisc_queue_drop,
	.init		=	tfifo_init,
	.reset		=	qdisc_reset_queue,
	.change		=	tfifo_init,
	.dump		=	tfifo_dump,
};

static int netem_init(struct Qdisc *sch, struct nlattr *opt)
{
	struct netem_sched_data *q = qdisc_priv(sch);
	int ret;

	if (!opt)
		return -EINVAL;

	qdisc_watchdog_init(&q->watchdog, sch);

	q->qdisc = qdisc_create_dflt(sch->dev_queue, &tfifo_qdisc_ops,
				     TC_H_MAKE(sch->handle, 1));
	if (!q->qdisc) {
		pr_debug("netem: qdisc create failed\n");
		return -ENOMEM;
	}

	ret = netem_change(sch, opt);
	if (ret) {
		pr_debug("netem: change failed\n");
		qdisc_destroy(q->qdisc);
	}
	return ret;
}

static void netem_destroy(struct Qdisc *sch)
{
	struct netem_sched_data *q = qdisc_priv(sch);

	qdisc_watchdog_cancel(&q->watchdog);
	qdisc_destroy(q->qdisc);
	kfree(q->delay_dist);
}

static int netem_dump(struct Qdisc *sch, struct sk_buff *skb)
{
	const struct netem_sched_data *q = qdisc_priv(sch);
	unsigned char *b = skb_tail_pointer(skb);
	struct nlattr *nla = (struct nlattr *) b;
	struct tc_netem_qopt qopt;
	struct tc_netem_corr cor;
	struct tc_netem_reorder reorder;
	struct tc_netem_corrupt corrupt;

	qopt.latency = q->latency;
	qopt.jitter = q->jitter;
	qopt.limit = q->limit;
	qopt.loss = q->loss;
	qopt.gap = q->gap;
	qopt.duplicate = q->duplicate;
	NLA_PUT(skb, TCA_OPTIONS, sizeof(qopt), &qopt);

	cor.delay_corr = q->delay_cor.rho;
	cor.loss_corr = q->loss_cor.rho;
	cor.dup_corr = q->dup_cor.rho;
	NLA_PUT(skb, TCA_NETEM_CORR, sizeof(cor), &cor);

	reorder.probability = q->reorder;
	reorder.correlation = q->reorder_cor.rho;
	NLA_PUT(skb, TCA_NETEM_REORDER, sizeof(reorder), &reorder);

	corrupt.probability = q->corrupt;
	corrupt.correlation = q->corrupt_cor.rho;
	NLA_PUT(skb, TCA_NETEM_CORRUPT, sizeof(corrupt), &corrupt);

	nla->nla_len = skb_tail_pointer(skb) - b;

	return skb->len;

nla_put_failure:
	nlmsg_trim(skb, b);
	return -1;
}

static struct Qdisc_ops netem_qdisc_ops __read_mostly = {
	.id		=	"netem",
	.priv_size	=	sizeof(struct netem_sched_data),
	.enqueue	=	netem_enqueue,
	.dequeue	=	netem_dequeue,
	.peek		=	qdisc_peek_dequeued,
	.drop		=	netem_drop,
	.init		=	netem_init,
	.reset		=	netem_reset,
	.destroy	=	netem_destroy,
	.change		=	netem_change,
	.dump		=	netem_dump,
	.owner		=	THIS_MODULE,
};


static int __init netem_module_init(void)
{
	pr_info("netem: version " VERSION "\n");
	return register_qdisc(&netem_qdisc_ops);
}
static void __exit netem_module_exit(void)
{
	unregister_qdisc(&netem_qdisc_ops);
}
module_init(netem_module_init)
module_exit(netem_module_exit)
MODULE_LICENSE("GPL");
Commit	Line	Data
1da177e4 LT	1	/*
	2	* net/sched/sch_netem.c Network emulator
	3	*
	4	* This program is free software; you can redistribute it and/or
	5	* modify it under the terms of the GNU General Public License
	6	* as published by the Free Software Foundation; either version
798b6b19	7	* 2 of the License.
1da177e4 LT	8	*
1da177e4 LT	9	* Many of the algorithms and ideas for this came from
10297b99	10	* NIST Net which is not copyrighted.
1da177e4 LT	11	*
	12	* Authors: Stephen Hemminger <shemminger@osdl.org>
	13	* Catalin(ux aka Dino) BOIE <catab at umbrella dot ro>
	14	*/
	15
1da177e4	16	#include <linux/module.h>
5a0e3ad6	17	#include <linux/slab.h>
1da177e4 LT	18	#include <linux/types.h>
	19	#include <linux/kernel.h>
	20	#include <linux/errno.h>
1da177e4 LT	21	#include <linux/skbuff.h>
	22	#include <linux/rtnetlink.h>
	23
dc5fc579	24	#include <net/netlink.h>
1da177e4 LT	25	#include <net/pkt_sched.h>
1da177e4 LT	26
c865e5d9	27	#define VERSION "1.2"
eb229c4c	28
1da177e4 LT	29	/* Network Emulation Queuing algorithm.
	30	====================================
	31
	32	Sources: [1] Mark Carson, Darrin Santay, "NIST Net - A Linux-based
	33	Network Emulation Tool
	34	[2] Luigi Rizzo, DummyNet for FreeBSD
	35
	36	----------------------------------------------------------------
	37
	38	This started out as a simple way to delay outgoing packets to
	39	test TCP but has grown to include most of the functionality
	40	of a full blown network emulator like NISTnet. It can delay
	41	packets and add random jitter (and correlation). The random
	42	distribution can be loaded from a table as well to provide
	43	normal, Pareto, or experimental curves. Packet loss,
	44	duplication, and reordering can also be emulated.
	45
	46	This qdisc does not do classification that can be handled in
	47	layering other disciplines. It does not need to do bandwidth
	48	control either since that can be handled by using token
	49	bucket or other rate control.
1da177e4 LT	50	*/
	51
	52	struct netem_sched_data {
	53	struct Qdisc *qdisc;
59cb5c67	54	struct qdisc_watchdog watchdog;
1da177e4	55
b407621c SH	56	psched_tdiff_t latency;
	57	psched_tdiff_t jitter;
	58
1da177e4 LT	59	u32 loss;
	60	u32 limit;
	61	u32 counter;
	62	u32 gap;
1da177e4	63	u32 duplicate;
0dca51d3	64	u32 reorder;
c865e5d9	65	u32 corrupt;
1da177e4 LT	66
1da177e4 LT	67	struct crndstate {
b407621c SH	68	u32 last;
b407621c SH	69	u32 rho;
c865e5d9	70	} delay_cor, loss_cor, dup_cor, reorder_cor, corrupt_cor;
1da177e4 LT	71
	72	struct disttable {
	73	u32 size;
	74	s16 table[0];
	75	} *delay_dist;
	76	};
	77
	78	/* Time stamp put into socket buffer control block */
	79	struct netem_skb_cb {
	80	psched_time_t time_to_send;
	81	};
	82
5f86173b JK	83	static inline struct netem_skb_cb netem_skb_cb(struct sk_buff skb)
5f86173b JK	84	{
175f9c1b JK	85	BUILD_BUG_ON(sizeof(skb->cb) <
	86	sizeof(struct qdisc_skb_cb) + sizeof(struct netem_skb_cb));
	87	return (struct netem_skb_cb *)qdisc_skb_cb(skb)->data;
5f86173b JK	88	}
5f86173b JK	89
1da177e4 LT	90	/* init_crandom - initialize correlated random number generator
	91	* Use entropy source for initial seed.
	92	*/
	93	static void init_crandom(struct crndstate *state, unsigned long rho)
	94	{
	95	state->rho = rho;
	96	state->last = net_random();
	97	}
	98
	99	/* get_crandom - correlated random number generator
	100	* Next number depends on last value.
	101	* rho is scaled to avoid floating point.
	102	*/
b407621c	103	static u32 get_crandom(struct crndstate *state)
1da177e4 LT	104	{
	105	u64 value, rho;
	106	unsigned long answer;
	107
bb2f8cc0	108	if (state->rho == 0) /* no correlation */
1da177e4 LT	109	return net_random();
	110
	111	value = net_random();
	112	rho = (u64)state->rho + 1;
	113	answer = (value * ((1ull<<32) - rho) + state->last * rho) >> 32;
	114	state->last = answer;
	115	return answer;
	116	}
	117
	118	/* tabledist - return a pseudo-randomly distributed value with mean mu and
	119	* std deviation sigma. Uses table lookup to approximate the desired
	120	* distribution, and a uniformly-distributed pseudo-random source.
	121	*/
b407621c SH	122	static psched_tdiff_t tabledist(psched_tdiff_t mu, psched_tdiff_t sigma,
	123	struct crndstate *state,
	124	const struct disttable *dist)
1da177e4	125	{
b407621c SH	126	psched_tdiff_t x;
	127	long t;
	128	u32 rnd;
1da177e4 LT	129
	130	if (sigma == 0)
	131	return mu;
	132
	133	rnd = get_crandom(state);
	134
	135	/* default uniform distribution */
10297b99	136	if (dist == NULL)
1da177e4 LT	137	return (rnd % (2*sigma)) - sigma + mu;
	138
	139	t = dist->table[rnd % dist->size];
	140	x = (sigma % NETEM_DIST_SCALE) * t;
	141	if (x >= 0)
	142	x += NETEM_DIST_SCALE/2;
	143	else
	144	x -= NETEM_DIST_SCALE/2;
	145
	146	return x / NETEM_DIST_SCALE + (sigma / NETEM_DIST_SCALE) * t + mu;
	147	}
	148
0afb51e7 SH	149	/*
	150	* Insert one skb into qdisc.
	151	* Note: parent depends on return value to account for queue length.
	152	* NET_XMIT_DROP: queue length didn't change.
	153	* NET_XMIT_SUCCESS: one skb was queued.
	154	*/
1da177e4 LT	155	static int netem_enqueue(struct sk_buff skb, struct Qdisc sch)
	156	{
	157	struct netem_sched_data *q = qdisc_priv(sch);
89e1df74 GC	158	/* We don't fill cb now as skb_unshare() may invalidate it */
89e1df74 GC	159	struct netem_skb_cb *cb;
0afb51e7	160	struct sk_buff *skb2;
1da177e4	161	int ret;
0afb51e7	162	int count = 1;
1da177e4	163
771018e7	164	pr_debug("netem_enqueue skb=%p\n", skb);
1da177e4	165
0afb51e7 SH	166	/* Random duplication */
	167	if (q->duplicate && q->duplicate >= get_crandom(&q->dup_cor))
	168	++count;
	169
1da177e4	170	/* Random packet drop 0 => none, ~0 => all */
0afb51e7 SH	171	if (q->loss && q->loss >= get_crandom(&q->loss_cor))
	172	--count;
	173
	174	if (count == 0) {
1da177e4 LT	175	sch->qstats.drops++;
1da177e4 LT	176	kfree_skb(skb);
c27f339a	177	return NET_XMIT_SUCCESS \| __NET_XMIT_BYPASS;
1da177e4 LT	178	}
1da177e4 LT	179
4e8a5201 DM	180	skb_orphan(skb);
4e8a5201 DM	181
0afb51e7 SH	182	/*
	183	* If we need to duplicate packet, then re-insert at top of the
	184	* qdisc tree, since parent queuer expects that only one
	185	* skb will be queued.
	186	*/
	187	if (count > 1 && (skb2 = skb_clone(skb, GFP_ATOMIC)) != NULL) {
7698b4fc	188	struct Qdisc *rootq = qdisc_root(sch);
0afb51e7 SH	189	u32 dupsave = q->duplicate; /* prevent duplicating a dup... */
	190	q->duplicate = 0;
	191
5f86173b	192	qdisc_enqueue_root(skb2, rootq);
0afb51e7	193	q->duplicate = dupsave;
1da177e4 LT	194	}
1da177e4 LT	195
c865e5d9 SH	196	/*
	197	* Randomized packet corruption.
	198	* Make copy if needed since we are modifying
	199	* If packet is going to be hardware checksummed, then
	200	* do it now in software before we mangle it.
	201	*/
	202	if (q->corrupt && q->corrupt >= get_crandom(&q->corrupt_cor)) {
f64f9e71 JP	203	if (!(skb = skb_unshare(skb, GFP_ATOMIC)) \|\|
	204	(skb->ip_summed == CHECKSUM_PARTIAL &&
	205	skb_checksum_help(skb))) {
c865e5d9 SH	206	sch->qstats.drops++;
	207	return NET_XMIT_DROP;
	208	}
	209
	210	skb->data[net_random() % skb_headlen(skb)] ^= 1<<(net_random() % 8);
	211	}
	212
5f86173b	213	cb = netem_skb_cb(skb);
f64f9e71 JP	214	if (q->gap == 0 \|\| /* not doing reordering */
	215	q->counter < q->gap \|\| /* inside last reordering gap */
	216	q->reorder < get_crandom(&q->reorder_cor)) {
0f9f32ac	217	psched_time_t now;
07aaa115 SH	218	psched_tdiff_t delay;
	219
	220	delay = tabledist(q->latency, q->jitter,
	221	&q->delay_cor, q->delay_dist);
	222
3bebcda2	223	now = psched_get_time();
7c59e25f	224	cb->time_to_send = now + delay;
1da177e4	225	++q->counter;
5f86173b	226	ret = qdisc_enqueue(skb, q->qdisc);
1da177e4	227	} else {
10297b99	228	/*
0dca51d3 SH	229	* Do re-ordering by putting one out of N packets at the front
	230	* of the queue.
	231	*/
3bebcda2	232	cb->time_to_send = psched_get_time();
0dca51d3	233	q->counter = 0;
8ba25dad JP	234
	235	__skb_queue_head(&q->qdisc->q, skb);
	236	q->qdisc->qstats.backlog += qdisc_pkt_len(skb);
	237	q->qdisc->qstats.requeues++;
	238	ret = NET_XMIT_SUCCESS;
1da177e4 LT	239	}
	240
	241	if (likely(ret == NET_XMIT_SUCCESS)) {
	242	sch->q.qlen++;
bfe0d029	243	qdisc_bstats_update(sch, skb);
378a2f09	244	} else if (net_xmit_drop_count(ret)) {
1da177e4	245	sch->qstats.drops++;
378a2f09	246	}
1da177e4	247
d5d75cd6	248	pr_debug("netem: enqueue ret %d\n", ret);
1da177e4 LT	249	return ret;
	250	}
	251
1da177e4 LT	252	static unsigned int netem_drop(struct Qdisc* sch)
	253	{
	254	struct netem_sched_data *q = qdisc_priv(sch);
6d037a26	255	unsigned int len = 0;
1da177e4	256
6d037a26	257	if (q->qdisc->ops->drop && (len = q->qdisc->ops->drop(q->qdisc)) != 0) {
1da177e4 LT	258	sch->q.qlen--;
	259	sch->qstats.drops++;
	260	}
	261	return len;
	262	}
	263
1da177e4 LT	264	static struct sk_buff netem_dequeue(struct Qdisc sch)
	265	{
	266	struct netem_sched_data *q = qdisc_priv(sch);
	267	struct sk_buff *skb;
	268
11274e5a SH	269	if (sch->flags & TCQ_F_THROTTLED)
	270	return NULL;
	271
03c05f0d	272	skb = q->qdisc->ops->peek(q->qdisc);
771018e7	273	if (skb) {
5f86173b	274	const struct netem_skb_cb *cb = netem_skb_cb(skb);
3bebcda2	275	psched_time_t now = psched_get_time();
0f9f32ac SH	276
0f9f32ac SH	277	/* if more time remaining? */
104e0878	278	if (cb->time_to_send <= now) {
77be155c JP	279	skb = qdisc_dequeue_peeked(q->qdisc);
77be155c JP	280	if (unlikely(!skb))
03c05f0d JP	281	return NULL;
03c05f0d JP	282
8caf1539 JP	283	#ifdef CONFIG_NET_CLS_ACT
	284	/*
	285	* If it's at ingress let's pretend the delay is
	286	* from the network (tstamp will be updated).
	287	*/
	288	if (G_TC_FROM(skb->tc_verd) & AT_INGRESS)
	289	skb->tstamp.tv64 = 0;
	290	#endif
0f9f32ac SH	291	pr_debug("netem_dequeue: return skb=%p\n", skb);
0f9f32ac SH	292	sch->q.qlen--;
0f9f32ac	293	return skb;
07aaa115	294	}
11274e5a	295
11274e5a	296	qdisc_watchdog_schedule(&q->watchdog, cb->time_to_send);
0f9f32ac SH	297	}
	298
	299	return NULL;
1da177e4 LT	300	}
1da177e4 LT	301
1da177e4 LT	302	static void netem_reset(struct Qdisc *sch)
	303	{
	304	struct netem_sched_data *q = qdisc_priv(sch);
	305
	306	qdisc_reset(q->qdisc);
1da177e4	307	sch->q.qlen = 0;
59cb5c67	308	qdisc_watchdog_cancel(&q->watchdog);
1da177e4 LT	309	}
1da177e4 LT	310
1da177e4 LT	311	/*
	312	* Distribution data is a variable size payload containing
	313	* signed 16 bit values.
	314	*/
1e90474c	315	static int get_dist_table(struct Qdisc sch, const struct nlattr attr)
1da177e4 LT	316	{
1da177e4 LT	317	struct netem_sched_data *q = qdisc_priv(sch);
1e90474c PM	318	unsigned long n = nla_len(attr)/sizeof(__s16);
1e90474c PM	319	const __s16 *data = nla_data(attr);
7698b4fc	320	spinlock_t *root_lock;
1da177e4 LT	321	struct disttable *d;
	322	int i;
	323
	324	if (n > 65536)
	325	return -EINVAL;
	326
	327	d = kmalloc(sizeof(d) + nsizeof(d->table[0]), GFP_KERNEL);
	328	if (!d)
	329	return -ENOMEM;
	330
	331	d->size = n;
	332	for (i = 0; i < n; i++)
	333	d->table[i] = data[i];
10297b99	334
102396ae	335	root_lock = qdisc_root_sleeping_lock(sch);
7698b4fc DM	336
7698b4fc DM	337	spin_lock_bh(root_lock);
b94c8afc PM	338	kfree(q->delay_dist);
b94c8afc PM	339	q->delay_dist = d;
7698b4fc	340	spin_unlock_bh(root_lock);
1da177e4 LT	341	return 0;
	342	}
	343
265eb67f	344	static void get_correlation(struct Qdisc sch, const struct nlattr attr)
1da177e4 LT	345	{
1da177e4 LT	346	struct netem_sched_data *q = qdisc_priv(sch);
1e90474c	347	const struct tc_netem_corr *c = nla_data(attr);
1da177e4	348
1da177e4 LT	349	init_crandom(&q->delay_cor, c->delay_corr);
	350	init_crandom(&q->loss_cor, c->loss_corr);
	351	init_crandom(&q->dup_cor, c->dup_corr);
1da177e4 LT	352	}
1da177e4 LT	353
265eb67f	354	static void get_reorder(struct Qdisc sch, const struct nlattr attr)
0dca51d3 SH	355	{
0dca51d3 SH	356	struct netem_sched_data *q = qdisc_priv(sch);
1e90474c	357	const struct tc_netem_reorder *r = nla_data(attr);
0dca51d3	358
0dca51d3 SH	359	q->reorder = r->probability;
0dca51d3 SH	360	init_crandom(&q->reorder_cor, r->correlation);
0dca51d3 SH	361	}
0dca51d3 SH	362
265eb67f	363	static void get_corrupt(struct Qdisc sch, const struct nlattr attr)
c865e5d9 SH	364	{
c865e5d9 SH	365	struct netem_sched_data *q = qdisc_priv(sch);
1e90474c	366	const struct tc_netem_corrupt *r = nla_data(attr);
c865e5d9	367
c865e5d9 SH	368	q->corrupt = r->probability;
c865e5d9 SH	369	init_crandom(&q->corrupt_cor, r->correlation);
c865e5d9 SH	370	}
c865e5d9 SH	371
27a3421e PM	372	static const struct nla_policy netem_policy[TCA_NETEM_MAX + 1] = {
	373	[TCA_NETEM_CORR] = { .len = sizeof(struct tc_netem_corr) },
	374	[TCA_NETEM_REORDER] = { .len = sizeof(struct tc_netem_reorder) },
	375	[TCA_NETEM_CORRUPT] = { .len = sizeof(struct tc_netem_corrupt) },
	376	};
	377
2c10b32b TG	378	static int parse_attr(struct nlattr tb[], int maxtype, struct nlattr nla,
	379	const struct nla_policy *policy, int len)
	380	{
	381	int nested_len = nla_len(nla) - NLA_ALIGN(len);
	382
	383	if (nested_len < 0)
	384	return -EINVAL;
	385	if (nested_len >= nla_attr_size(0))
	386	return nla_parse(tb, maxtype, nla_data(nla) + NLA_ALIGN(len),
	387	nested_len, policy);
	388	memset(tb, 0, sizeof(struct nlattr ) (maxtype + 1));
	389	return 0;
	390	}
	391
c865e5d9	392	/* Parse netlink message to set options */
1e90474c	393	static int netem_change(struct Qdisc sch, struct nlattr opt)
1da177e4 LT	394	{
1da177e4 LT	395	struct netem_sched_data *q = qdisc_priv(sch);
b03f4672	396	struct nlattr *tb[TCA_NETEM_MAX + 1];
1da177e4 LT	397	struct tc_netem_qopt *qopt;
1da177e4 LT	398	int ret;
10297b99	399
b03f4672	400	if (opt == NULL)
1da177e4 LT	401	return -EINVAL;
1da177e4 LT	402
2c10b32b TG	403	qopt = nla_data(opt);
2c10b32b TG	404	ret = parse_attr(tb, TCA_NETEM_MAX, opt, netem_policy, sizeof(*qopt));
b03f4672 PM	405	if (ret < 0)
	406	return ret;
	407
fb0305ce	408	ret = fifo_set_limit(q->qdisc, qopt->limit);
1da177e4 LT	409	if (ret) {
	410	pr_debug("netem: can't set fifo limit\n");
	411	return ret;
	412	}
10297b99	413
1da177e4 LT	414	q->latency = qopt->latency;
	415	q->jitter = qopt->jitter;
	416	q->limit = qopt->limit;
	417	q->gap = qopt->gap;
0dca51d3	418	q->counter = 0;
1da177e4 LT	419	q->loss = qopt->loss;
	420	q->duplicate = qopt->duplicate;
	421
bb2f8cc0 SH	422	/* for compatibility with earlier versions.
bb2f8cc0 SH	423	* if gap is set, need to assume 100% probability
0dca51d3	424	*/
a362e0a7 SH	425	if (q->gap)
a362e0a7 SH	426	q->reorder = ~0;
0dca51d3	427
265eb67f SH	428	if (tb[TCA_NETEM_CORR])
265eb67f SH	429	get_correlation(sch, tb[TCA_NETEM_CORR]);
1da177e4	430
b03f4672 PM	431	if (tb[TCA_NETEM_DELAY_DIST]) {
	432	ret = get_dist_table(sch, tb[TCA_NETEM_DELAY_DIST]);
	433	if (ret)
	434	return ret;
	435	}
c865e5d9	436
265eb67f SH	437	if (tb[TCA_NETEM_REORDER])
265eb67f SH	438	get_reorder(sch, tb[TCA_NETEM_REORDER]);
1da177e4	439
265eb67f SH	440	if (tb[TCA_NETEM_CORRUPT])
265eb67f SH	441	get_corrupt(sch, tb[TCA_NETEM_CORRUPT]);
1da177e4 LT	442
	443	return 0;
	444	}
	445
300ce174 SH	446	/*
	447	* Special case version of FIFO queue for use by netem.
	448	* It queues in order based on timestamps in skb's
	449	*/
	450	struct fifo_sched_data {
	451	u32 limit;
075aa573	452	psched_time_t oldest;
300ce174 SH	453	};
	454
	455	static int tfifo_enqueue(struct sk_buff nskb, struct Qdisc sch)
	456	{
	457	struct fifo_sched_data *q = qdisc_priv(sch);
	458	struct sk_buff_head *list = &sch->q;
5f86173b	459	psched_time_t tnext = netem_skb_cb(nskb)->time_to_send;
300ce174 SH	460	struct sk_buff *skb;
	461
	462	if (likely(skb_queue_len(list) < q->limit)) {
075aa573	463	/* Optimize for add at tail */
104e0878	464	if (likely(skb_queue_empty(list) \|\| tnext >= q->oldest)) {
075aa573 SH	465	q->oldest = tnext;
	466	return qdisc_enqueue_tail(nskb, sch);
	467	}
	468
300ce174	469	skb_queue_reverse_walk(list, skb) {
5f86173b	470	const struct netem_skb_cb *cb = netem_skb_cb(skb);
300ce174	471
104e0878	472	if (tnext >= cb->time_to_send)
300ce174 SH	473	break;
	474	}
	475
	476	__skb_queue_after(list, skb, nskb);
	477
0abf77e5	478	sch->qstats.backlog += qdisc_pkt_len(nskb);
bfe0d029	479	qdisc_bstats_update(sch, nskb);
300ce174 SH	480
	481	return NET_XMIT_SUCCESS;
	482	}
	483
075aa573	484	return qdisc_reshape_fail(nskb, sch);
300ce174 SH	485	}
300ce174 SH	486
1e90474c	487	static int tfifo_init(struct Qdisc sch, struct nlattr opt)
300ce174 SH	488	{
	489	struct fifo_sched_data *q = qdisc_priv(sch);
	490
	491	if (opt) {
1e90474c PM	492	struct tc_fifo_qopt *ctl = nla_data(opt);
1e90474c PM	493	if (nla_len(opt) < sizeof(*ctl))
300ce174 SH	494	return -EINVAL;
	495
	496	q->limit = ctl->limit;
	497	} else
5ce2d488	498	q->limit = max_t(u32, qdisc_dev(sch)->tx_queue_len, 1);
300ce174	499
a084980d	500	q->oldest = PSCHED_PASTPERFECT;
300ce174 SH	501	return 0;
	502	}
	503
	504	static int tfifo_dump(struct Qdisc sch, struct sk_buff skb)
	505	{
	506	struct fifo_sched_data *q = qdisc_priv(sch);
	507	struct tc_fifo_qopt opt = { .limit = q->limit };
	508
1e90474c	509	NLA_PUT(skb, TCA_OPTIONS, sizeof(opt), &opt);
300ce174 SH	510	return skb->len;
300ce174 SH	511
1e90474c	512	nla_put_failure:
300ce174 SH	513	return -1;
	514	}
	515
20fea08b	516	static struct Qdisc_ops tfifo_qdisc_ops __read_mostly = {
300ce174 SH	517	.id = "tfifo",
	518	.priv_size = sizeof(struct fifo_sched_data),
	519	.enqueue = tfifo_enqueue,
	520	.dequeue = qdisc_dequeue_head,
8e3af978	521	.peek = qdisc_peek_head,
300ce174 SH	522	.drop = qdisc_queue_drop,
	523	.init = tfifo_init,
	524	.reset = qdisc_reset_queue,
	525	.change = tfifo_init,
	526	.dump = tfifo_dump,
	527	};
	528
1e90474c	529	static int netem_init(struct Qdisc sch, struct nlattr opt)
1da177e4 LT	530	{
	531	struct netem_sched_data *q = qdisc_priv(sch);
	532	int ret;
	533
	534	if (!opt)
	535	return -EINVAL;
	536
59cb5c67	537	qdisc_watchdog_init(&q->watchdog, sch);
1da177e4	538
3511c913	539	q->qdisc = qdisc_create_dflt(sch->dev_queue, &tfifo_qdisc_ops,
9f9afec4	540	TC_H_MAKE(sch->handle, 1));
1da177e4 LT	541	if (!q->qdisc) {
	542	pr_debug("netem: qdisc create failed\n");
	543	return -ENOMEM;
	544	}
	545
	546	ret = netem_change(sch, opt);
	547	if (ret) {
	548	pr_debug("netem: change failed\n");
	549	qdisc_destroy(q->qdisc);
	550	}
	551	return ret;
	552	}
	553
	554	static void netem_destroy(struct Qdisc *sch)
	555	{
	556	struct netem_sched_data *q = qdisc_priv(sch);
	557
59cb5c67	558	qdisc_watchdog_cancel(&q->watchdog);
1da177e4 LT	559	qdisc_destroy(q->qdisc);
	560	kfree(q->delay_dist);
	561	}
	562
	563	static int netem_dump(struct Qdisc sch, struct sk_buff skb)
	564	{
	565	const struct netem_sched_data *q = qdisc_priv(sch);
27a884dc	566	unsigned char *b = skb_tail_pointer(skb);
1e90474c	567	struct nlattr nla = (struct nlattr ) b;
1da177e4 LT	568	struct tc_netem_qopt qopt;
1da177e4 LT	569	struct tc_netem_corr cor;
0dca51d3	570	struct tc_netem_reorder reorder;
c865e5d9	571	struct tc_netem_corrupt corrupt;
1da177e4 LT	572
	573	qopt.latency = q->latency;
	574	qopt.jitter = q->jitter;
	575	qopt.limit = q->limit;
	576	qopt.loss = q->loss;
	577	qopt.gap = q->gap;
	578	qopt.duplicate = q->duplicate;
1e90474c	579	NLA_PUT(skb, TCA_OPTIONS, sizeof(qopt), &qopt);
1da177e4 LT	580
	581	cor.delay_corr = q->delay_cor.rho;
	582	cor.loss_corr = q->loss_cor.rho;
	583	cor.dup_corr = q->dup_cor.rho;
1e90474c	584	NLA_PUT(skb, TCA_NETEM_CORR, sizeof(cor), &cor);
0dca51d3 SH	585
	586	reorder.probability = q->reorder;
	587	reorder.correlation = q->reorder_cor.rho;
1e90474c	588	NLA_PUT(skb, TCA_NETEM_REORDER, sizeof(reorder), &reorder);
0dca51d3	589
c865e5d9 SH	590	corrupt.probability = q->corrupt;
c865e5d9 SH	591	corrupt.correlation = q->corrupt_cor.rho;
1e90474c	592	NLA_PUT(skb, TCA_NETEM_CORRUPT, sizeof(corrupt), &corrupt);
c865e5d9	593
1e90474c	594	nla->nla_len = skb_tail_pointer(skb) - b;
1da177e4 LT	595
	596	return skb->len;
	597
1e90474c	598	nla_put_failure:
dc5fc579	599	nlmsg_trim(skb, b);
1da177e4 LT	600	return -1;
	601	}
	602
20fea08b	603	static struct Qdisc_ops netem_qdisc_ops __read_mostly = {
1da177e4	604	.id = "netem",
1da177e4 LT	605	.priv_size = sizeof(struct netem_sched_data),
	606	.enqueue = netem_enqueue,
	607	.dequeue = netem_dequeue,
77be155c	608	.peek = qdisc_peek_dequeued,
1da177e4 LT	609	.drop = netem_drop,
	610	.init = netem_init,
	611	.reset = netem_reset,
	612	.destroy = netem_destroy,
	613	.change = netem_change,
	614	.dump = netem_dump,
	615	.owner = THIS_MODULE,
	616	};
	617
	618
	619	static int __init netem_module_init(void)
	620	{
eb229c4c	621	pr_info("netem: version " VERSION "\n");
1da177e4 LT	622	return register_qdisc(&netem_qdisc_ops);
	623	}
	624	static void __exit netem_module_exit(void)
	625	{
	626	unregister_qdisc(&netem_qdisc_ops);
	627	}
	628	module_init(netem_module_init)
	629	module_exit(netem_module_exit)
	630	MODULE_LICENSE("GPL");