1/*
2 * net/sched/sch_prio.c	Simple 3-band priority "scheduler".
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, <kuznet@ms2.inr.ac.ru>
10 * Fixes:       19990609: J Hadi Salim <hadi@nortelnetworks.com>:
11 *              Init --  EINVAL when opt undefined
12 */
13
14#include <linux/module.h>
15#include <asm/uaccess.h>
16#include <asm/system.h>
17#include <linux/bitops.h>
18#include <linux/types.h>
19#include <linux/kernel.h>
20#include <linux/string.h>
21#include <linux/mm.h>
22#include <linux/socket.h>
23#include <linux/sockios.h>
24#include <linux/in.h>
25#include <linux/errno.h>
26#include <linux/interrupt.h>
27#include <linux/if_ether.h>
28#include <linux/inet.h>
29#include <linux/netdevice.h>
30#include <linux/etherdevice.h>
31#include <linux/notifier.h>
32#include <net/ip.h>
33#include <net/route.h>
34#include <linux/skbuff.h>
35#include <net/netlink.h>
36#include <net/sock.h>
37#include <net/pkt_sched.h>
38
39
40struct prio_sched_data
41{
42	int bands;
43	struct tcf_proto *filter_list;
44	u8  prio2band[TC_PRIO_MAX+1];
45	struct Qdisc *queues[TCQ_PRIO_BANDS];
46};
47
48
49static struct Qdisc *
50prio_classify(struct sk_buff *skb, struct Qdisc *sch, int *qerr)
51{
52	struct prio_sched_data *q = qdisc_priv(sch);
53	u32 band = skb->priority;
54	struct tcf_result res;
55
56	*qerr = NET_XMIT_BYPASS;
57	if (TC_H_MAJ(skb->priority) != sch->handle) {
58#ifdef CONFIG_NET_CLS_ACT
59		switch (tc_classify(skb, q->filter_list, &res)) {
60		case TC_ACT_STOLEN:
61		case TC_ACT_QUEUED:
62			*qerr = NET_XMIT_SUCCESS;
63		case TC_ACT_SHOT:
64			return NULL;
65		}
66
67		if (!q->filter_list ) {
68#else
69		if (!q->filter_list || tc_classify(skb, q->filter_list, &res)) {
70#endif
71			if (TC_H_MAJ(band))
72				band = 0;
73			return q->queues[q->prio2band[band&TC_PRIO_MAX]];
74		}
75		band = res.classid;
76	}
77	band = TC_H_MIN(band) - 1;
78	if (band >= q->bands)
79		return q->queues[q->prio2band[0]];
80
81	return q->queues[band];
82}
83
84static int
85prio_enqueue(struct sk_buff *skb, struct Qdisc *sch)
86{
87	struct Qdisc *qdisc;
88	int ret;
89
90	qdisc = prio_classify(skb, sch, &ret);
91#ifdef CONFIG_NET_CLS_ACT
92	if (qdisc == NULL) {
93
94		if (ret == NET_XMIT_BYPASS)
95			sch->qstats.drops++;
96		kfree_skb(skb);
97		return ret;
98	}
99#endif
100
101	if ((ret = qdisc->enqueue(skb, qdisc)) == NET_XMIT_SUCCESS) {
102		sch->bstats.bytes += skb->len;
103		sch->bstats.packets++;
104		sch->q.qlen++;
105		return NET_XMIT_SUCCESS;
106	}
107	sch->qstats.drops++;
108	return ret;
109}
110
111
112static int
113prio_requeue(struct sk_buff *skb, struct Qdisc* sch)
114{
115	struct Qdisc *qdisc;
116	int ret;
117
118	qdisc = prio_classify(skb, sch, &ret);
119#ifdef CONFIG_NET_CLS_ACT
120	if (qdisc == NULL) {
121		if (ret == NET_XMIT_BYPASS)
122			sch->qstats.drops++;
123		kfree_skb(skb);
124		return ret;
125	}
126#endif
127
128	if ((ret = qdisc->ops->requeue(skb, qdisc)) == NET_XMIT_SUCCESS) {
129		sch->q.qlen++;
130		sch->qstats.requeues++;
131		return 0;
132	}
133	sch->qstats.drops++;
134	return NET_XMIT_DROP;
135}
136
137
138static struct sk_buff *
139prio_dequeue(struct Qdisc* sch)
140{
141	struct sk_buff *skb;
142	struct prio_sched_data *q = qdisc_priv(sch);
143	int prio;
144	struct Qdisc *qdisc;
145
146	for (prio = 0; prio < q->bands; prio++) {
147		qdisc = q->queues[prio];
148		skb = qdisc->dequeue(qdisc);
149		if (skb) {
150			sch->q.qlen--;
151			return skb;
152		}
153	}
154	return NULL;
155
156}
157
158static unsigned int prio_drop(struct Qdisc* sch)
159{
160	struct prio_sched_data *q = qdisc_priv(sch);
161	int prio;
162	unsigned int len;
163	struct Qdisc *qdisc;
164
165	for (prio = q->bands-1; prio >= 0; prio--) {
166		qdisc = q->queues[prio];
167		if (qdisc->ops->drop && (len = qdisc->ops->drop(qdisc)) != 0) {
168			sch->q.qlen--;
169			return len;
170		}
171	}
172	return 0;
173}
174
175
176static void
177prio_reset(struct Qdisc* sch)
178{
179	int prio;
180	struct prio_sched_data *q = qdisc_priv(sch);
181
182	for (prio=0; prio<q->bands; prio++)
183		qdisc_reset(q->queues[prio]);
184	sch->q.qlen = 0;
185}
186
187static void
188prio_destroy(struct Qdisc* sch)
189{
190	int prio;
191	struct prio_sched_data *q = qdisc_priv(sch);
192
193	tcf_destroy_chain(q->filter_list);
194	for (prio=0; prio<q->bands; prio++)
195		qdisc_destroy(q->queues[prio]);
196}
197
198static int prio_tune(struct Qdisc *sch, struct rtattr *opt)
199{
200	struct prio_sched_data *q = qdisc_priv(sch);
201	struct tc_prio_qopt *qopt = RTA_DATA(opt);
202	int i;
203
204	if (opt->rta_len < RTA_LENGTH(sizeof(*qopt)))
205		return -EINVAL;
206	if (qopt->bands > TCQ_PRIO_BANDS || qopt->bands < 2)
207		return -EINVAL;
208
209	for (i=0; i<=TC_PRIO_MAX; i++) {
210		if (qopt->priomap[i] >= qopt->bands)
211			return -EINVAL;
212	}
213
214	sch_tree_lock(sch);
215	q->bands = qopt->bands;
216	memcpy(q->prio2band, qopt->priomap, TC_PRIO_MAX+1);
217
218	for (i=q->bands; i<TCQ_PRIO_BANDS; i++) {
219		struct Qdisc *child = xchg(&q->queues[i], &noop_qdisc);
220		if (child != &noop_qdisc) {
221			qdisc_tree_decrease_qlen(child, child->q.qlen);
222			qdisc_destroy(child);
223		}
224	}
225	sch_tree_unlock(sch);
226
227	for (i=0; i<q->bands; i++) {
228		if (q->queues[i] == &noop_qdisc) {
229			struct Qdisc *child;
230			child = qdisc_create_dflt(sch->dev, &pfifo_qdisc_ops,
231						  TC_H_MAKE(sch->handle, i + 1));
232			if (child) {
233				sch_tree_lock(sch);
234				child = xchg(&q->queues[i], child);
235
236				if (child != &noop_qdisc) {
237					qdisc_tree_decrease_qlen(child,
238								 child->q.qlen);
239					qdisc_destroy(child);
240				}
241				sch_tree_unlock(sch);
242			}
243		}
244	}
245	return 0;
246}
247
248static int prio_init(struct Qdisc *sch, struct rtattr *opt)
249{
250	struct prio_sched_data *q = qdisc_priv(sch);
251	int i;
252
253	for (i=0; i<TCQ_PRIO_BANDS; i++)
254		q->queues[i] = &noop_qdisc;
255
256	if (opt == NULL) {
257		return -EINVAL;
258	} else {
259		int err;
260
261		if ((err= prio_tune(sch, opt)) != 0)
262			return err;
263	}
264	return 0;
265}
266
267static int prio_dump(struct Qdisc *sch, struct sk_buff *skb)
268{
269	struct prio_sched_data *q = qdisc_priv(sch);
270	unsigned char *b = skb_tail_pointer(skb);
271	struct tc_prio_qopt opt;
272
273	opt.bands = q->bands;
274	memcpy(&opt.priomap, q->prio2band, TC_PRIO_MAX+1);
275	RTA_PUT(skb, TCA_OPTIONS, sizeof(opt), &opt);
276	return skb->len;
277
278rtattr_failure:
279	nlmsg_trim(skb, b);
280	return -1;
281}
282
283static int prio_graft(struct Qdisc *sch, unsigned long arg, struct Qdisc *new,
284		      struct Qdisc **old)
285{
286	struct prio_sched_data *q = qdisc_priv(sch);
287	unsigned long band = arg - 1;
288
289	if (band >= q->bands)
290		return -EINVAL;
291
292	if (new == NULL)
293		new = &noop_qdisc;
294
295	sch_tree_lock(sch);
296	*old = q->queues[band];
297	q->queues[band] = new;
298	qdisc_tree_decrease_qlen(*old, (*old)->q.qlen);
299	qdisc_reset(*old);
300	sch_tree_unlock(sch);
301
302	return 0;
303}
304
305static struct Qdisc *
306prio_leaf(struct Qdisc *sch, unsigned long arg)
307{
308	struct prio_sched_data *q = qdisc_priv(sch);
309	unsigned long band = arg - 1;
310
311	if (band >= q->bands)
312		return NULL;
313
314	return q->queues[band];
315}
316
317static unsigned long prio_get(struct Qdisc *sch, u32 classid)
318{
319	struct prio_sched_data *q = qdisc_priv(sch);
320	unsigned long band = TC_H_MIN(classid);
321
322	if (band - 1 >= q->bands)
323		return 0;
324	return band;
325}
326
327static unsigned long prio_bind(struct Qdisc *sch, unsigned long parent, u32 classid)
328{
329	return prio_get(sch, classid);
330}
331
332
333static void prio_put(struct Qdisc *q, unsigned long cl)
334{
335	return;
336}
337
338static int prio_change(struct Qdisc *sch, u32 handle, u32 parent, struct rtattr **tca, unsigned long *arg)
339{
340	unsigned long cl = *arg;
341	struct prio_sched_data *q = qdisc_priv(sch);
342
343	if (cl - 1 > q->bands)
344		return -ENOENT;
345	return 0;
346}
347
348static int prio_delete(struct Qdisc *sch, unsigned long cl)
349{
350	struct prio_sched_data *q = qdisc_priv(sch);
351	if (cl - 1 > q->bands)
352		return -ENOENT;
353	return 0;
354}
355
356
357static int prio_dump_class(struct Qdisc *sch, unsigned long cl, struct sk_buff *skb,
358			   struct tcmsg *tcm)
359{
360	struct prio_sched_data *q = qdisc_priv(sch);
361
362	if (cl - 1 > q->bands)
363		return -ENOENT;
364	tcm->tcm_handle |= TC_H_MIN(cl);
365	if (q->queues[cl-1])
366		tcm->tcm_info = q->queues[cl-1]->handle;
367	return 0;
368}
369
370static int prio_dump_class_stats(struct Qdisc *sch, unsigned long cl,
371				 struct gnet_dump *d)
372{
373	struct prio_sched_data *q = qdisc_priv(sch);
374	struct Qdisc *cl_q;
375
376	cl_q = q->queues[cl - 1];
377	if (gnet_stats_copy_basic(d, &cl_q->bstats) < 0 ||
378	    gnet_stats_copy_queue(d, &cl_q->qstats) < 0)
379		return -1;
380
381	return 0;
382}
383
384static void prio_walk(struct Qdisc *sch, struct qdisc_walker *arg)
385{
386	struct prio_sched_data *q = qdisc_priv(sch);
387	int prio;
388
389	if (arg->stop)
390		return;
391
392	for (prio = 0; prio < q->bands; prio++) {
393		if (arg->count < arg->skip) {
394			arg->count++;
395			continue;
396		}
397		if (arg->fn(sch, prio+1, arg) < 0) {
398			arg->stop = 1;
399			break;
400		}
401		arg->count++;
402	}
403}
404
405static struct tcf_proto ** prio_find_tcf(struct Qdisc *sch, unsigned long cl)
406{
407	struct prio_sched_data *q = qdisc_priv(sch);
408
409	if (cl)
410		return NULL;
411	return &q->filter_list;
412}
413
414static struct Qdisc_class_ops prio_class_ops = {
415	.graft		=	prio_graft,
416	.leaf		=	prio_leaf,
417	.get		=	prio_get,
418	.put		=	prio_put,
419	.change		=	prio_change,
420	.delete		=	prio_delete,
421	.walk		=	prio_walk,
422	.tcf_chain	=	prio_find_tcf,
423	.bind_tcf	=	prio_bind,
424	.unbind_tcf	=	prio_put,
425	.dump		=	prio_dump_class,
426	.dump_stats	=	prio_dump_class_stats,
427};
428
429static struct Qdisc_ops prio_qdisc_ops = {
430	.next		=	NULL,
431	.cl_ops		=	&prio_class_ops,
432	.id		=	"prio",
433	.priv_size	=	sizeof(struct prio_sched_data),
434	.enqueue	=	prio_enqueue,
435	.dequeue	=	prio_dequeue,
436	.requeue	=	prio_requeue,
437	.drop		=	prio_drop,
438	.init		=	prio_init,
439	.reset		=	prio_reset,
440	.destroy	=	prio_destroy,
441	.change		=	prio_tune,
442	.dump		=	prio_dump,
443	.owner		=	THIS_MODULE,
444};
445
446static int __init prio_module_init(void)
447{
448	return register_qdisc(&prio_qdisc_ops);
449}
450
451static void __exit prio_module_exit(void)
452{
453	unregister_qdisc(&prio_qdisc_ops);
454}
455
456module_init(prio_module_init)
457module_exit(prio_module_exit)
458
459MODULE_LICENSE("GPL");
460