[linux.git] / net / sched / sch_sfq.c

/*
 * net/sched/sch_sfq.c	Stochastic Fairness Queueing discipline.
 *
 *		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, or (at your option) any later version.
 *
 * Authors:	Alexey Kuznetsov, <[email protected]>
 */

#include <linux/module.h>
#include <linux/types.h>
#include <linux/kernel.h>
#include <linux/jiffies.h>
#include <linux/string.h>
#include <linux/in.h>
#include <linux/errno.h>
#include <linux/init.h>
#include <linux/ipv6.h>
#include <linux/skbuff.h>
#include <linux/jhash.h>
#include <linux/slab.h>
#include <net/ip.h>
#include <net/netlink.h>
#include <net/pkt_sched.h>


/*	Stochastic Fairness Queuing algorithm.
	=======================================

	Source:
	Paul E. McKenney "Stochastic Fairness Queuing",
	IEEE INFOCOMM'90 Proceedings, San Francisco, 1990.

	Paul E. McKenney "Stochastic Fairness Queuing",
	"Interworking: Research and Experience", v.2, 1991, p.113-131.


	See also:
	M. Shreedhar and George Varghese "Efficient Fair
	Queuing using Deficit Round Robin", Proc. SIGCOMM 95.


	This is not the thing that is usually called (W)FQ nowadays.
	It does not use any timestamp mechanism, but instead
	processes queues in round-robin order.

	ADVANTAGE:

	- It is very cheap. Both CPU and memory requirements are minimal.

	DRAWBACKS:

	- "Stochastic" -> It is not 100% fair.
	When hash collisions occur, several flows are considered as one.

	- "Round-robin" -> It introduces larger delays than virtual clock
	based schemes, and should not be used for isolating interactive
	traffic	from non-interactive. It means, that this scheduler
	should be used as leaf of CBQ or P3, which put interactive traffic
	to higher priority band.

	We still need true WFQ for top level CSZ, but using WFQ
	for the best effort traffic is absolutely pointless:
	SFQ is superior for this purpose.

	IMPLEMENTATION:
	This implementation limits maximal queue length to 128;
	maximal mtu to 2^15-1; number of hash buckets to 1024.
	The only goal of this restrictions was that all data
	fit into one 4K page :-). Struct sfq_sched_data is
	organized in anti-cache manner: all the data for a bucket
	are scattered over different locations. This is not good,
	but it allowed me to put it into 4K.

	It is easy to increase these values, but not in flight.  */

#define SFQ_DEPTH		128
#define SFQ_HASH_DIVISOR	1024

/* This type should contain at least SFQ_DEPTH*2 values */
typedef unsigned char sfq_index;

struct sfq_head
{
	sfq_index	next;
	sfq_index	prev;
};

struct sfq_sched_data
{
/* Parameters */
	int		perturb_period;
	unsigned	quantum;	/* Allotment per round: MUST BE >= MTU */
	int		limit;

/* Variables */
	struct tcf_proto *filter_list;
	struct timer_list perturb_timer;
	u32		perturbation;
	sfq_index	tail;		/* Index of current slot in round */
	sfq_index	max_depth;	/* Maximal depth */

	sfq_index	ht[SFQ_HASH_DIVISOR];	/* Hash table */
	sfq_index	next[SFQ_DEPTH];	/* Active slots link */
	short		allot[SFQ_DEPTH];	/* Current allotment per slot */
	unsigned short	hash[SFQ_DEPTH];	/* Hash value indexed by slots */
	struct sk_buff_head	qs[SFQ_DEPTH];		/* Slot queue */
	struct sfq_head	dep[SFQ_DEPTH*2];	/* Linked list of slots, indexed by depth */
};

static __inline__ unsigned sfq_fold_hash(struct sfq_sched_data *q, u32 h, u32 h1)
{
	return jhash_2words(h, h1, q->perturbation) & (SFQ_HASH_DIVISOR - 1);
}

static unsigned sfq_hash(struct sfq_sched_data *q, struct sk_buff *skb)
{
	u32 h, h2;

	switch (skb->protocol) {
	case htons(ETH_P_IP):
	{
		const struct iphdr *iph = ip_hdr(skb);
		h = iph->daddr;
		h2 = iph->saddr ^ iph->protocol;
		if (!(iph->frag_off&htons(IP_MF|IP_OFFSET)) &&
		    (iph->protocol == IPPROTO_TCP ||
		     iph->protocol == IPPROTO_UDP ||
		     iph->protocol == IPPROTO_UDPLITE ||
		     iph->protocol == IPPROTO_SCTP ||
		     iph->protocol == IPPROTO_DCCP ||
		     iph->protocol == IPPROTO_ESP))
			h2 ^= *(((u32*)iph) + iph->ihl);
		break;
	}
	case htons(ETH_P_IPV6):
	{
		struct ipv6hdr *iph = ipv6_hdr(skb);
		h = iph->daddr.s6_addr32[3];
		h2 = iph->saddr.s6_addr32[3] ^ iph->nexthdr;
		if (iph->nexthdr == IPPROTO_TCP ||
		    iph->nexthdr == IPPROTO_UDP ||
		    iph->nexthdr == IPPROTO_UDPLITE ||
		    iph->nexthdr == IPPROTO_SCTP ||
		    iph->nexthdr == IPPROTO_DCCP ||
		    iph->nexthdr == IPPROTO_ESP)
			h2 ^= *(u32*)&iph[1];
		break;
	}
	default:
		h = (unsigned long)skb_dst(skb) ^ skb->protocol;
		h2 = (unsigned long)skb->sk;
	}

	return sfq_fold_hash(q, h, h2);
}

static unsigned int sfq_classify(struct sk_buff *skb, struct Qdisc *sch,
				 int *qerr)
{
	struct sfq_sched_data *q = qdisc_priv(sch);
	struct tcf_result res;
	int result;

	if (TC_H_MAJ(skb->priority) == sch->handle &&
	    TC_H_MIN(skb->priority) > 0 &&
	    TC_H_MIN(skb->priority) <= SFQ_HASH_DIVISOR)
		return TC_H_MIN(skb->priority);

	if (!q->filter_list)
		return sfq_hash(q, skb) + 1;

	*qerr = NET_XMIT_SUCCESS | __NET_XMIT_BYPASS;
	result = tc_classify(skb, q->filter_list, &res);
	if (result >= 0) {
#ifdef CONFIG_NET_CLS_ACT
		switch (result) {
		case TC_ACT_STOLEN:
		case TC_ACT_QUEUED:
			*qerr = NET_XMIT_SUCCESS | __NET_XMIT_STOLEN;
		case TC_ACT_SHOT:
			return 0;
		}
#endif
		if (TC_H_MIN(res.classid) <= SFQ_HASH_DIVISOR)
			return TC_H_MIN(res.classid);
	}
	return 0;
}

static inline void sfq_link(struct sfq_sched_data *q, sfq_index x)
{
	sfq_index p, n;
	int d = q->qs[x].qlen + SFQ_DEPTH;

	p = d;
	n = q->dep[d].next;
	q->dep[x].next = n;
	q->dep[x].prev = p;
	q->dep[p].next = q->dep[n].prev = x;
}

static inline void sfq_dec(struct sfq_sched_data *q, sfq_index x)
{
	sfq_index p, n;

	n = q->dep[x].next;
	p = q->dep[x].prev;
	q->dep[p].next = n;
	q->dep[n].prev = p;

	if (n == p && q->max_depth == q->qs[x].qlen + 1)
		q->max_depth--;

	sfq_link(q, x);
}

static inline void sfq_inc(struct sfq_sched_data *q, sfq_index x)
{
	sfq_index p, n;
	int d;

	n = q->dep[x].next;
	p = q->dep[x].prev;
	q->dep[p].next = n;
	q->dep[n].prev = p;
	d = q->qs[x].qlen;
	if (q->max_depth < d)
		q->max_depth = d;

	sfq_link(q, x);
}

static unsigned int sfq_drop(struct Qdisc *sch)
{
	struct sfq_sched_data *q = qdisc_priv(sch);
	sfq_index d = q->max_depth;
	struct sk_buff *skb;
	unsigned int len;

	/* Queue is full! Find the longest slot and
	   drop a packet from it */

	if (d > 1) {
		sfq_index x = q->dep[d + SFQ_DEPTH].next;
		skb = q->qs[x].prev;
		len = qdisc_pkt_len(skb);
		__skb_unlink(skb, &q->qs[x]);
		kfree_skb(skb);
		sfq_dec(q, x);
		sch->q.qlen--;
		sch->qstats.drops++;
		sch->qstats.backlog -= len;
		return len;
	}

	if (d == 1) {
		/* It is difficult to believe, but ALL THE SLOTS HAVE LENGTH 1. */
		d = q->next[q->tail];
		q->next[q->tail] = q->next[d];
		q->allot[q->next[d]] += q->quantum;
		skb = q->qs[d].prev;
		len = qdisc_pkt_len(skb);
		__skb_unlink(skb, &q->qs[d]);
		kfree_skb(skb);
		sfq_dec(q, d);
		sch->q.qlen--;
		q->ht[q->hash[d]] = SFQ_DEPTH;
		sch->qstats.drops++;
		sch->qstats.backlog -= len;
		return len;
	}

	return 0;
}

static int
sfq_enqueue(struct sk_buff *skb, struct Qdisc *sch)
{
	struct sfq_sched_data *q = qdisc_priv(sch);
	unsigned int hash;
	sfq_index x;
	int uninitialized_var(ret);

	hash = sfq_classify(skb, sch, &ret);
	if (hash == 0) {
		if (ret & __NET_XMIT_BYPASS)
			sch->qstats.drops++;
		kfree_skb(skb);
		return ret;
	}
	hash--;

	x = q->ht[hash];
	if (x == SFQ_DEPTH) {
		q->ht[hash] = x = q->dep[SFQ_DEPTH].next;
		q->hash[x] = hash;
	}

	/* If selected queue has length q->limit, this means that
	 * all another queues are empty and that we do simple tail drop,
	 * i.e. drop _this_ packet.
	 */
	if (q->qs[x].qlen >= q->limit)
		return qdisc_drop(skb, sch);

	sch->qstats.backlog += qdisc_pkt_len(skb);
	__skb_queue_tail(&q->qs[x], skb);
	sfq_inc(q, x);
	if (q->qs[x].qlen == 1) {		/* The flow is new */
		if (q->tail == SFQ_DEPTH) {	/* It is the first flow */
			q->tail = x;
			q->next[x] = x;
			q->allot[x] = q->quantum;
		} else {
			q->next[x] = q->next[q->tail];
			q->next[q->tail] = x;
			q->tail = x;
		}
	}
	if (++sch->q.qlen <= q->limit) {
		sch->bstats.bytes += qdisc_pkt_len(skb);
		sch->bstats.packets++;
		return 0;
	}

	sfq_drop(sch);
	return NET_XMIT_CN;
}

static struct sk_buff *
sfq_peek(struct Qdisc *sch)
{
	struct sfq_sched_data *q = qdisc_priv(sch);
	sfq_index a;

	/* No active slots */
	if (q->tail == SFQ_DEPTH)
		return NULL;

	a = q->next[q->tail];
	return skb_peek(&q->qs[a]);
}

static struct sk_buff *
sfq_dequeue(struct Qdisc *sch)
{
	struct sfq_sched_data *q = qdisc_priv(sch);
	struct sk_buff *skb;
	sfq_index a, old_a;

	/* No active slots */
	if (q->tail == SFQ_DEPTH)
		return NULL;

	a = old_a = q->next[q->tail];

	/* Grab packet */
	skb = __skb_dequeue(&q->qs[a]);
	sfq_dec(q, a);
	sch->q.qlen--;
	sch->qstats.backlog -= qdisc_pkt_len(skb);

	/* Is the slot empty? */
	if (q->qs[a].qlen == 0) {
		q->ht[q->hash[a]] = SFQ_DEPTH;
		a = q->next[a];
		if (a == old_a) {
			q->tail = SFQ_DEPTH;
			return skb;
		}
		q->next[q->tail] = a;
		q->allot[a] += q->quantum;
	} else if ((q->allot[a] -= qdisc_pkt_len(skb)) <= 0) {
		q->tail = a;
		a = q->next[a];
		q->allot[a] += q->quantum;
	}
	return skb;
}

static void
sfq_reset(struct Qdisc *sch)
{
	struct sk_buff *skb;

	while ((skb = sfq_dequeue(sch)) != NULL)
		kfree_skb(skb);
}

static void sfq_perturbation(unsigned long arg)
{
	struct Qdisc *sch = (struct Qdisc *)arg;
	struct sfq_sched_data *q = qdisc_priv(sch);

	q->perturbation = net_random();

	if (q->perturb_period)
		mod_timer(&q->perturb_timer, jiffies + q->perturb_period);
}

static int sfq_change(struct Qdisc *sch, struct nlattr *opt)
{
	struct sfq_sched_data *q = qdisc_priv(sch);
	struct tc_sfq_qopt *ctl = nla_data(opt);
	unsigned int qlen;

	if (opt->nla_len < nla_attr_size(sizeof(*ctl)))
		return -EINVAL;

	sch_tree_lock(sch);
	q->quantum = ctl->quantum ? : psched_mtu(qdisc_dev(sch));
	q->perturb_period = ctl->perturb_period * HZ;
	if (ctl->limit)
		q->limit = min_t(u32, ctl->limit, SFQ_DEPTH - 1);

	qlen = sch->q.qlen;
	while (sch->q.qlen > q->limit)
		sfq_drop(sch);
	qdisc_tree_decrease_qlen(sch, qlen - sch->q.qlen);

	del_timer(&q->perturb_timer);
	if (q->perturb_period) {
		mod_timer(&q->perturb_timer, jiffies + q->perturb_period);
		q->perturbation = net_random();
	}
	sch_tree_unlock(sch);
	return 0;
}

static int sfq_init(struct Qdisc *sch, struct nlattr *opt)
{
	struct sfq_sched_data *q = qdisc_priv(sch);
	int i;

	q->perturb_timer.function = sfq_perturbation;
	q->perturb_timer.data = (unsigned long)sch;
	init_timer_deferrable(&q->perturb_timer);

	for (i = 0; i < SFQ_HASH_DIVISOR; i++)
		q->ht[i] = SFQ_DEPTH;

	for (i = 0; i < SFQ_DEPTH; i++) {
		skb_queue_head_init(&q->qs[i]);
		q->dep[i + SFQ_DEPTH].next = i + SFQ_DEPTH;
		q->dep[i + SFQ_DEPTH].prev = i + SFQ_DEPTH;
	}

	q->limit = SFQ_DEPTH - 1;
	q->max_depth = 0;
	q->tail = SFQ_DEPTH;
	if (opt == NULL) {
		q->quantum = psched_mtu(qdisc_dev(sch));
		q->perturb_period = 0;
		q->perturbation = net_random();
	} else {
		int err = sfq_change(sch, opt);
		if (err)
			return err;
	}

	for (i = 0; i < SFQ_DEPTH; i++)
		sfq_link(q, i);
	return 0;
}

static void sfq_destroy(struct Qdisc *sch)
{
	struct sfq_sched_data *q = qdisc_priv(sch);

	tcf_destroy_chain(&q->filter_list);
	q->perturb_period = 0;
	del_timer_sync(&q->perturb_timer);
}

static int sfq_dump(struct Qdisc *sch, struct sk_buff *skb)
{
	struct sfq_sched_data *q = qdisc_priv(sch);
	unsigned char *b = skb_tail_pointer(skb);
	struct tc_sfq_qopt opt;

	opt.quantum = q->quantum;
	opt.perturb_period = q->perturb_period / HZ;

	opt.limit = q->limit;
	opt.divisor = SFQ_HASH_DIVISOR;
	opt.flows = q->limit;

	NLA_PUT(skb, TCA_OPTIONS, sizeof(opt), &opt);

	return skb->len;

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

static unsigned long sfq_get(struct Qdisc *sch, u32 classid)
{
	return 0;
}

static struct tcf_proto **sfq_find_tcf(struct Qdisc *sch, unsigned long cl)
{
	struct sfq_sched_data *q = qdisc_priv(sch);

	if (cl)
		return NULL;
	return &q->filter_list;
}

static int sfq_dump_class(struct Qdisc *sch, unsigned long cl,
			  struct sk_buff *skb, struct tcmsg *tcm)
{
	tcm->tcm_handle |= TC_H_MIN(cl);
	return 0;
}

static int sfq_dump_class_stats(struct Qdisc *sch, unsigned long cl,
				struct gnet_dump *d)
{
	struct sfq_sched_data *q = qdisc_priv(sch);
	sfq_index idx = q->ht[cl-1];
	struct gnet_stats_queue qs = { .qlen = q->qs[idx].qlen };
	struct tc_sfq_xstats xstats = { .allot = q->allot[idx] };

	if (gnet_stats_copy_queue(d, &qs) < 0)
		return -1;
	return gnet_stats_copy_app(d, &xstats, sizeof(xstats));
}

static void sfq_walk(struct Qdisc *sch, struct qdisc_walker *arg)
{
	struct sfq_sched_data *q = qdisc_priv(sch);
	unsigned int i;

	if (arg->stop)
		return;

	for (i = 0; i < SFQ_HASH_DIVISOR; i++) {
		if (q->ht[i] == SFQ_DEPTH ||
		    arg->count < arg->skip) {
			arg->count++;
			continue;
		}
		if (arg->fn(sch, i + 1, arg) < 0) {
			arg->stop = 1;
			break;
		}
		arg->count++;
	}
}

static const struct Qdisc_class_ops sfq_class_ops = {
	.get		=	sfq_get,
	.tcf_chain	=	sfq_find_tcf,
	.dump		=	sfq_dump_class,
	.dump_stats	=	sfq_dump_class_stats,
	.walk		=	sfq_walk,
};

static struct Qdisc_ops sfq_qdisc_ops __read_mostly = {
	.cl_ops		=	&sfq_class_ops,
	.id		=	"sfq",
	.priv_size	=	sizeof(struct sfq_sched_data),
	.enqueue	=	sfq_enqueue,
	.dequeue	=	sfq_dequeue,
	.peek		=	sfq_peek,
	.drop		=	sfq_drop,
	.init		=	sfq_init,
	.reset		=	sfq_reset,
	.destroy	=	sfq_destroy,
	.change		=	NULL,
	.dump		=	sfq_dump,
	.owner		=	THIS_MODULE,
};

static int __init sfq_module_init(void)
{
	return register_qdisc(&sfq_qdisc_ops);
}
static void __exit sfq_module_exit(void)
{
	unregister_qdisc(&sfq_qdisc_ops);
}
module_init(sfq_module_init)
module_exit(sfq_module_exit)
MODULE_LICENSE("GPL");
Commit	Line	Data
1da177e4 LT	1	/*
	2	* net/sched/sch_sfq.c Stochastic Fairness Queueing discipline.
	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
	7	* 2 of the License, or (at your option) any later version.
	8	*
	9	* Authors: Alexey Kuznetsov, <[email protected]>
	10	*/
	11
1da177e4	12	#include <linux/module.h>
1da177e4 LT	13	#include <linux/types.h>
	14	#include <linux/kernel.h>
	15	#include <linux/jiffies.h>
	16	#include <linux/string.h>
1da177e4 LT	17	#include <linux/in.h>
1da177e4 LT	18	#include <linux/errno.h>
1da177e4	19	#include <linux/init.h>
1da177e4	20	#include <linux/ipv6.h>
1da177e4	21	#include <linux/skbuff.h>
32740ddc	22	#include <linux/jhash.h>
5a0e3ad6	23	#include <linux/slab.h>
0ba48053 PM	24	#include <net/ip.h>
0ba48053 PM	25	#include <net/netlink.h>
1da177e4 LT	26	#include <net/pkt_sched.h>
	27
	28
	29	/* Stochastic Fairness Queuing algorithm.
	30	=======================================
	31
	32	Source:
	33	Paul E. McKenney "Stochastic Fairness Queuing",
	34	IEEE INFOCOMM'90 Proceedings, San Francisco, 1990.
	35
	36	Paul E. McKenney "Stochastic Fairness Queuing",
	37	"Interworking: Research and Experience", v.2, 1991, p.113-131.
	38
	39
	40	See also:
	41	M. Shreedhar and George Varghese "Efficient Fair
	42	Queuing using Deficit Round Robin", Proc. SIGCOMM 95.
	43
	44
10297b99	45	This is not the thing that is usually called (W)FQ nowadays.
1da177e4 LT	46	It does not use any timestamp mechanism, but instead
	47	processes queues in round-robin order.
	48
	49	ADVANTAGE:
	50
	51	- It is very cheap. Both CPU and memory requirements are minimal.
	52
	53	DRAWBACKS:
	54
10297b99	55	- "Stochastic" -> It is not 100% fair.
1da177e4 LT	56	When hash collisions occur, several flows are considered as one.
	57
	58	- "Round-robin" -> It introduces larger delays than virtual clock
	59	based schemes, and should not be used for isolating interactive
	60	traffic from non-interactive. It means, that this scheduler
	61	should be used as leaf of CBQ or P3, which put interactive traffic
	62	to higher priority band.
	63
	64	We still need true WFQ for top level CSZ, but using WFQ
	65	for the best effort traffic is absolutely pointless:
	66	SFQ is superior for this purpose.
	67
	68	IMPLEMENTATION:
	69	This implementation limits maximal queue length to 128;
	70	maximal mtu to 2^15-1; number of hash buckets to 1024.
	71	The only goal of this restrictions was that all data
	72	fit into one 4K page :-). Struct sfq_sched_data is
	73	organized in anti-cache manner: all the data for a bucket
	74	are scattered over different locations. This is not good,
	75	but it allowed me to put it into 4K.
	76
	77	It is easy to increase these values, but not in flight. */
	78
	79	#define SFQ_DEPTH 128
	80	#define SFQ_HASH_DIVISOR 1024
	81
	82	/* This type should contain at least SFQ_DEPTH2 values /
	83	typedef unsigned char sfq_index;
	84
	85	struct sfq_head
	86	{
	87	sfq_index next;
	88	sfq_index prev;
	89	};
	90
	91	struct sfq_sched_data
	92	{
	93	/* Parameters */
	94	int perturb_period;
	95	unsigned quantum; /* Allotment per round: MUST BE >= MTU */
	96	int limit;
	97
	98	/* Variables */
7d2681a6	99	struct tcf_proto *filter_list;
1da177e4	100	struct timer_list perturb_timer;
32740ddc	101	u32 perturbation;
1da177e4 LT	102	sfq_index tail; /* Index of current slot in round */
	103	sfq_index max_depth; /* Maximal depth */
	104
	105	sfq_index ht[SFQ_HASH_DIVISOR]; /* Hash table */
	106	sfq_index next[SFQ_DEPTH]; /* Active slots link */
	107	short allot[SFQ_DEPTH]; /* Current allotment per slot */
	108	unsigned short hash[SFQ_DEPTH]; /* Hash value indexed by slots */
	109	struct sk_buff_head qs[SFQ_DEPTH]; /* Slot queue */
	110	struct sfq_head dep[SFQ_DEPTH2]; / Linked list of slots, indexed by depth */
	111	};
	112
	113	static __inline__ unsigned sfq_fold_hash(struct sfq_sched_data *q, u32 h, u32 h1)
	114	{
32740ddc	115	return jhash_2words(h, h1, q->perturbation) & (SFQ_HASH_DIVISOR - 1);
1da177e4 LT	116	}
	117
	118	static unsigned sfq_hash(struct sfq_sched_data q, struct sk_buff skb)
	119	{
	120	u32 h, h2;
	121
	122	switch (skb->protocol) {
60678040	123	case htons(ETH_P_IP):
1da177e4	124	{
eddc9ec5	125	const struct iphdr *iph = ip_hdr(skb);
1da177e4	126	h = iph->daddr;
6f9e98f7	127	h2 = iph->saddr ^ iph->protocol;
1da177e4 LT	128	if (!(iph->frag_off&htons(IP_MF\|IP_OFFSET)) &&
	129	(iph->protocol == IPPROTO_TCP \|\|
	130	iph->protocol == IPPROTO_UDP \|\|
a8d0f952	131	iph->protocol == IPPROTO_UDPLITE \|\|
ae82af54 PM	132	iph->protocol == IPPROTO_SCTP \|\|
ae82af54 PM	133	iph->protocol == IPPROTO_DCCP \|\|
1da177e4 LT	134	iph->protocol == IPPROTO_ESP))
	135	h2 ^= (((u32)iph) + iph->ihl);
	136	break;
	137	}
60678040	138	case htons(ETH_P_IPV6):
1da177e4	139	{
0660e03f	140	struct ipv6hdr *iph = ipv6_hdr(skb);
1da177e4	141	h = iph->daddr.s6_addr32[3];
6f9e98f7	142	h2 = iph->saddr.s6_addr32[3] ^ iph->nexthdr;
1da177e4 LT	143	if (iph->nexthdr == IPPROTO_TCP \|\|
1da177e4 LT	144	iph->nexthdr == IPPROTO_UDP \|\|
a8d0f952	145	iph->nexthdr == IPPROTO_UDPLITE \|\|
ae82af54 PM	146	iph->nexthdr == IPPROTO_SCTP \|\|
ae82af54 PM	147	iph->nexthdr == IPPROTO_DCCP \|\|
1da177e4 LT	148	iph->nexthdr == IPPROTO_ESP)
	149	h2 ^= (u32)&iph[1];
	150	break;
	151	}
	152	default:
adf30907	153	h = (unsigned long)skb_dst(skb) ^ skb->protocol;
6f9e98f7	154	h2 = (unsigned long)skb->sk;
1da177e4	155	}
6f9e98f7	156
1da177e4 LT	157	return sfq_fold_hash(q, h, h2);
	158	}
	159
7d2681a6 PM	160	static unsigned int sfq_classify(struct sk_buff skb, struct Qdisc sch,
	161	int *qerr)
	162	{
	163	struct sfq_sched_data *q = qdisc_priv(sch);
	164	struct tcf_result res;
	165	int result;
	166
	167	if (TC_H_MAJ(skb->priority) == sch->handle &&
	168	TC_H_MIN(skb->priority) > 0 &&
	169	TC_H_MIN(skb->priority) <= SFQ_HASH_DIVISOR)
	170	return TC_H_MIN(skb->priority);
	171
	172	if (!q->filter_list)
	173	return sfq_hash(q, skb) + 1;
	174
c27f339a	175	*qerr = NET_XMIT_SUCCESS \| __NET_XMIT_BYPASS;
7d2681a6 PM	176	result = tc_classify(skb, q->filter_list, &res);
	177	if (result >= 0) {
	178	#ifdef CONFIG_NET_CLS_ACT
	179	switch (result) {
	180	case TC_ACT_STOLEN:
	181	case TC_ACT_QUEUED:
378a2f09	182	*qerr = NET_XMIT_SUCCESS \| __NET_XMIT_STOLEN;
7d2681a6 PM	183	case TC_ACT_SHOT:
	184	return 0;
	185	}
	186	#endif
	187	if (TC_H_MIN(res.classid) <= SFQ_HASH_DIVISOR)
	188	return TC_H_MIN(res.classid);
	189	}
	190	return 0;
	191	}
	192
1da177e4 LT	193	static inline void sfq_link(struct sfq_sched_data *q, sfq_index x)
	194	{
	195	sfq_index p, n;
	196	int d = q->qs[x].qlen + SFQ_DEPTH;
	197
	198	p = d;
	199	n = q->dep[d].next;
	200	q->dep[x].next = n;
	201	q->dep[x].prev = p;
	202	q->dep[p].next = q->dep[n].prev = x;
	203	}
	204
	205	static inline void sfq_dec(struct sfq_sched_data *q, sfq_index x)
	206	{
	207	sfq_index p, n;
	208
	209	n = q->dep[x].next;
	210	p = q->dep[x].prev;
	211	q->dep[p].next = n;
	212	q->dep[n].prev = p;
	213
	214	if (n == p && q->max_depth == q->qs[x].qlen + 1)
	215	q->max_depth--;
	216
	217	sfq_link(q, x);
	218	}
	219
	220	static inline void sfq_inc(struct sfq_sched_data *q, sfq_index x)
	221	{
	222	sfq_index p, n;
	223	int d;
	224
	225	n = q->dep[x].next;
	226	p = q->dep[x].prev;
	227	q->dep[p].next = n;
	228	q->dep[n].prev = p;
	229	d = q->qs[x].qlen;
	230	if (q->max_depth < d)
	231	q->max_depth = d;
	232
	233	sfq_link(q, x);
	234	}
	235
	236	static unsigned int sfq_drop(struct Qdisc *sch)
	237	{
	238	struct sfq_sched_data *q = qdisc_priv(sch);
	239	sfq_index d = q->max_depth;
	240	struct sk_buff *skb;
	241	unsigned int len;
	242
	243	/* Queue is full! Find the longest slot and
	244	drop a packet from it */
	245
	246	if (d > 1) {
6f9e98f7	247	sfq_index x = q->dep[d + SFQ_DEPTH].next;
1da177e4	248	skb = q->qs[x].prev;
0abf77e5	249	len = qdisc_pkt_len(skb);
1da177e4 LT	250	__skb_unlink(skb, &q->qs[x]);
	251	kfree_skb(skb);
	252	sfq_dec(q, x);
	253	sch->q.qlen--;
	254	sch->qstats.drops++;
f5539eb8	255	sch->qstats.backlog -= len;
1da177e4 LT	256	return len;
	257	}
	258
	259	if (d == 1) {
	260	/* It is difficult to believe, but ALL THE SLOTS HAVE LENGTH 1. */
	261	d = q->next[q->tail];
	262	q->next[q->tail] = q->next[d];
	263	q->allot[q->next[d]] += q->quantum;
	264	skb = q->qs[d].prev;
0abf77e5	265	len = qdisc_pkt_len(skb);
1da177e4 LT	266	__skb_unlink(skb, &q->qs[d]);
	267	kfree_skb(skb);
	268	sfq_dec(q, d);
	269	sch->q.qlen--;
	270	q->ht[q->hash[d]] = SFQ_DEPTH;
	271	sch->qstats.drops++;
f5539eb8	272	sch->qstats.backlog -= len;
1da177e4 LT	273	return len;
	274	}
	275
	276	return 0;
	277	}
	278
	279	static int
6f9e98f7	280	sfq_enqueue(struct sk_buff skb, struct Qdisc sch)
1da177e4 LT	281	{
1da177e4 LT	282	struct sfq_sched_data *q = qdisc_priv(sch);
7d2681a6	283	unsigned int hash;
1da177e4	284	sfq_index x;
7f3ff4f6	285	int uninitialized_var(ret);
7d2681a6 PM	286
	287	hash = sfq_classify(skb, sch, &ret);
	288	if (hash == 0) {
c27f339a	289	if (ret & __NET_XMIT_BYPASS)
7d2681a6 PM	290	sch->qstats.drops++;
	291	kfree_skb(skb);
	292	return ret;
	293	}
	294	hash--;
1da177e4 LT	295
	296	x = q->ht[hash];
	297	if (x == SFQ_DEPTH) {
	298	q->ht[hash] = x = q->dep[SFQ_DEPTH].next;
	299	q->hash[x] = hash;
	300	}
6f9e98f7	301
32740ddc AK	302	/* If selected queue has length q->limit, this means that
	303	* all another queues are empty and that we do simple tail drop,
	304	* i.e. drop _this_ packet.
	305	*/
	306	if (q->qs[x].qlen >= q->limit)
	307	return qdisc_drop(skb, sch);
	308
0abf77e5	309	sch->qstats.backlog += qdisc_pkt_len(skb);
1da177e4 LT	310	__skb_queue_tail(&q->qs[x], skb);
	311	sfq_inc(q, x);
	312	if (q->qs[x].qlen == 1) { /* The flow is new */
	313	if (q->tail == SFQ_DEPTH) { /* It is the first flow */
	314	q->tail = x;
	315	q->next[x] = x;
	316	q->allot[x] = q->quantum;
	317	} else {
	318	q->next[x] = q->next[q->tail];
	319	q->next[q->tail] = x;
	320	q->tail = x;
	321	}
	322	}
5588b40d	323	if (++sch->q.qlen <= q->limit) {
0abf77e5	324	sch->bstats.bytes += qdisc_pkt_len(skb);
1da177e4 LT	325	sch->bstats.packets++;
	326	return 0;
	327	}
	328
	329	sfq_drop(sch);
	330	return NET_XMIT_CN;
	331	}
	332
48a8f519 PM	333	static struct sk_buff *
	334	sfq_peek(struct Qdisc *sch)
	335	{
	336	struct sfq_sched_data *q = qdisc_priv(sch);
	337	sfq_index a;
1da177e4	338
48a8f519 PM	339	/* No active slots */
	340	if (q->tail == SFQ_DEPTH)
	341	return NULL;
1da177e4	342
48a8f519 PM	343	a = q->next[q->tail];
	344	return skb_peek(&q->qs[a]);
	345	}
1da177e4 LT	346
1da177e4 LT	347	static struct sk_buff *
6f9e98f7	348	sfq_dequeue(struct Qdisc *sch)
1da177e4 LT	349	{
	350	struct sfq_sched_data *q = qdisc_priv(sch);
	351	struct sk_buff *skb;
	352	sfq_index a, old_a;
	353
	354	/* No active slots */
	355	if (q->tail == SFQ_DEPTH)
	356	return NULL;
	357
	358	a = old_a = q->next[q->tail];
	359
	360	/* Grab packet */
	361	skb = __skb_dequeue(&q->qs[a]);
	362	sfq_dec(q, a);
	363	sch->q.qlen--;
0abf77e5	364	sch->qstats.backlog -= qdisc_pkt_len(skb);
1da177e4 LT	365
	366	/* Is the slot empty? */
	367	if (q->qs[a].qlen == 0) {
	368	q->ht[q->hash[a]] = SFQ_DEPTH;
	369	a = q->next[a];
	370	if (a == old_a) {
	371	q->tail = SFQ_DEPTH;
	372	return skb;
	373	}
	374	q->next[q->tail] = a;
	375	q->allot[a] += q->quantum;
0abf77e5	376	} else if ((q->allot[a] -= qdisc_pkt_len(skb)) <= 0) {
1da177e4 LT	377	q->tail = a;
	378	a = q->next[a];
	379	q->allot[a] += q->quantum;
	380	}
	381	return skb;
	382	}
	383
	384	static void
6f9e98f7	385	sfq_reset(struct Qdisc *sch)
1da177e4 LT	386	{
	387	struct sk_buff *skb;
	388
	389	while ((skb = sfq_dequeue(sch)) != NULL)
	390	kfree_skb(skb);
	391	}
	392
	393	static void sfq_perturbation(unsigned long arg)
	394	{
6f9e98f7	395	struct Qdisc sch = (struct Qdisc )arg;
1da177e4 LT	396	struct sfq_sched_data *q = qdisc_priv(sch);
1da177e4 LT	397
d46f8dd8	398	q->perturbation = net_random();
1da177e4	399
32740ddc AK	400	if (q->perturb_period)
32740ddc AK	401	mod_timer(&q->perturb_timer, jiffies + q->perturb_period);
1da177e4 LT	402	}
1da177e4 LT	403
1e90474c	404	static int sfq_change(struct Qdisc sch, struct nlattr opt)
1da177e4 LT	405	{
1da177e4 LT	406	struct sfq_sched_data *q = qdisc_priv(sch);
1e90474c	407	struct tc_sfq_qopt *ctl = nla_data(opt);
5e50da01	408	unsigned int qlen;
1da177e4	409
1e90474c	410	if (opt->nla_len < nla_attr_size(sizeof(*ctl)))
1da177e4 LT	411	return -EINVAL;
	412
	413	sch_tree_lock(sch);
5ce2d488	414	q->quantum = ctl->quantum ? : psched_mtu(qdisc_dev(sch));
6f9e98f7	415	q->perturb_period = ctl->perturb_period * HZ;
1da177e4	416	if (ctl->limit)
32740ddc	417	q->limit = min_t(u32, ctl->limit, SFQ_DEPTH - 1);
1da177e4	418
5e50da01	419	qlen = sch->q.qlen;
5588b40d	420	while (sch->q.qlen > q->limit)
1da177e4	421	sfq_drop(sch);
5e50da01	422	qdisc_tree_decrease_qlen(sch, qlen - sch->q.qlen);
1da177e4 LT	423
	424	del_timer(&q->perturb_timer);
	425	if (q->perturb_period) {
32740ddc	426	mod_timer(&q->perturb_timer, jiffies + q->perturb_period);
d46f8dd8	427	q->perturbation = net_random();
1da177e4 LT	428	}
	429	sch_tree_unlock(sch);
	430	return 0;
	431	}
	432
1e90474c	433	static int sfq_init(struct Qdisc sch, struct nlattr opt)
1da177e4 LT	434	{
	435	struct sfq_sched_data *q = qdisc_priv(sch);
	436	int i;
	437
d3e99483	438	q->perturb_timer.function = sfq_perturbation;
c19a28e1	439	q->perturb_timer.data = (unsigned long)sch;
d3e99483	440	init_timer_deferrable(&q->perturb_timer);
1da177e4	441
6f9e98f7	442	for (i = 0; i < SFQ_HASH_DIVISOR; i++)
1da177e4	443	q->ht[i] = SFQ_DEPTH;
6f9e98f7 SH	444
6f9e98f7 SH	445	for (i = 0; i < SFQ_DEPTH; i++) {
1da177e4	446	skb_queue_head_init(&q->qs[i]);
6f9e98f7 SH	447	q->dep[i + SFQ_DEPTH].next = i + SFQ_DEPTH;
6f9e98f7 SH	448	q->dep[i + SFQ_DEPTH].prev = i + SFQ_DEPTH;
1da177e4	449	}
6f9e98f7	450
32740ddc	451	q->limit = SFQ_DEPTH - 1;
1da177e4 LT	452	q->max_depth = 0;
	453	q->tail = SFQ_DEPTH;
	454	if (opt == NULL) {
5ce2d488	455	q->quantum = psched_mtu(qdisc_dev(sch));
1da177e4	456	q->perturb_period = 0;
d46f8dd8	457	q->perturbation = net_random();
1da177e4 LT	458	} else {
	459	int err = sfq_change(sch, opt);
	460	if (err)
	461	return err;
	462	}
6f9e98f7 SH	463
6f9e98f7 SH	464	for (i = 0; i < SFQ_DEPTH; i++)
1da177e4 LT	465	sfq_link(q, i);
	466	return 0;
	467	}
	468
	469	static void sfq_destroy(struct Qdisc *sch)
	470	{
	471	struct sfq_sched_data *q = qdisc_priv(sch);
7d2681a6	472
ff31ab56	473	tcf_destroy_chain(&q->filter_list);
980c478d JP	474	q->perturb_period = 0;
980c478d JP	475	del_timer_sync(&q->perturb_timer);
1da177e4 LT	476	}
	477
	478	static int sfq_dump(struct Qdisc sch, struct sk_buff skb)
	479	{
	480	struct sfq_sched_data *q = qdisc_priv(sch);
27a884dc	481	unsigned char *b = skb_tail_pointer(skb);
1da177e4 LT	482	struct tc_sfq_qopt opt;
	483
	484	opt.quantum = q->quantum;
6f9e98f7	485	opt.perturb_period = q->perturb_period / HZ;
1da177e4 LT	486
	487	opt.limit = q->limit;
	488	opt.divisor = SFQ_HASH_DIVISOR;
cdec7e50	489	opt.flows = q->limit;
1da177e4	490
1e90474c	491	NLA_PUT(skb, TCA_OPTIONS, sizeof(opt), &opt);
1da177e4 LT	492
	493	return skb->len;
	494
1e90474c	495	nla_put_failure:
dc5fc579	496	nlmsg_trim(skb, b);
1da177e4 LT	497	return -1;
	498	}
	499
7d2681a6 PM	500	static unsigned long sfq_get(struct Qdisc *sch, u32 classid)
	501	{
	502	return 0;
	503	}
	504
	505	static struct tcf_proto *sfq_find_tcf(struct Qdisc sch, unsigned long cl)
	506	{
	507	struct sfq_sched_data *q = qdisc_priv(sch);
	508
	509	if (cl)
	510	return NULL;
	511	return &q->filter_list;
	512	}
	513
94de78d1 PM	514	static int sfq_dump_class(struct Qdisc *sch, unsigned long cl,
	515	struct sk_buff skb, struct tcmsg tcm)
	516	{
	517	tcm->tcm_handle \|= TC_H_MIN(cl);
	518	return 0;
	519	}
	520
	521	static int sfq_dump_class_stats(struct Qdisc *sch, unsigned long cl,
	522	struct gnet_dump *d)
	523	{
	524	struct sfq_sched_data *q = qdisc_priv(sch);
	525	sfq_index idx = q->ht[cl-1];
	526	struct gnet_stats_queue qs = { .qlen = q->qs[idx].qlen };
	527	struct tc_sfq_xstats xstats = { .allot = q->allot[idx] };
	528
	529	if (gnet_stats_copy_queue(d, &qs) < 0)
	530	return -1;
	531	return gnet_stats_copy_app(d, &xstats, sizeof(xstats));
	532	}
	533
7d2681a6 PM	534	static void sfq_walk(struct Qdisc sch, struct qdisc_walker arg)
7d2681a6 PM	535	{
94de78d1 PM	536	struct sfq_sched_data *q = qdisc_priv(sch);
	537	unsigned int i;
	538
	539	if (arg->stop)
	540	return;
	541
	542	for (i = 0; i < SFQ_HASH_DIVISOR; i++) {
	543	if (q->ht[i] == SFQ_DEPTH \|\|
	544	arg->count < arg->skip) {
	545	arg->count++;
	546	continue;
	547	}
	548	if (arg->fn(sch, i + 1, arg) < 0) {
	549	arg->stop = 1;
	550	break;
	551	}
	552	arg->count++;
	553	}
7d2681a6 PM	554	}
	555
	556	static const struct Qdisc_class_ops sfq_class_ops = {
	557	.get = sfq_get,
7d2681a6	558	.tcf_chain = sfq_find_tcf,
94de78d1 PM	559	.dump = sfq_dump_class,
94de78d1 PM	560	.dump_stats = sfq_dump_class_stats,
7d2681a6 PM	561	.walk = sfq_walk,
	562	};
	563
20fea08b	564	static struct Qdisc_ops sfq_qdisc_ops __read_mostly = {
7d2681a6	565	.cl_ops = &sfq_class_ops,
1da177e4 LT	566	.id = "sfq",
	567	.priv_size = sizeof(struct sfq_sched_data),
	568	.enqueue = sfq_enqueue,
	569	.dequeue = sfq_dequeue,
48a8f519	570	.peek = sfq_peek,
1da177e4 LT	571	.drop = sfq_drop,
	572	.init = sfq_init,
	573	.reset = sfq_reset,
	574	.destroy = sfq_destroy,
	575	.change = NULL,
	576	.dump = sfq_dump,
	577	.owner = THIS_MODULE,
	578	};
	579
	580	static int __init sfq_module_init(void)
	581	{
	582	return register_qdisc(&sfq_qdisc_ops);
	583	}
10297b99	584	static void __exit sfq_module_exit(void)
1da177e4 LT	585	{
	586	unregister_qdisc(&sfq_qdisc_ops);
	587	}
	588	module_init(sfq_module_init)
	589	module_exit(sfq_module_exit)
	590	MODULE_LICENSE("GPL");