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