155648d23b7cb5a2ed3918f1a097a0fc1de0b42a
[linux-2.6.git] / net / sched / sch_multiq.c
1 /*
2  * Copyright (c) 2008, Intel Corporation.
3  *
4  * This program is free software; you can redistribute it and/or modify it
5  * under the terms and conditions of the GNU General Public License,
6  * version 2, as published by the Free Software Foundation.
7  *
8  * This program is distributed in the hope it will be useful, but WITHOUT
9  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
10  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
11  * more details.
12  *
13  * You should have received a copy of the GNU General Public License along with
14  * this program; if not, write to the Free Software Foundation, Inc., 59 Temple
15  * Place - Suite 330, Boston, MA 02111-1307 USA.
16  *
17  * Author: Alexander Duyck <alexander.h.duyck@intel.com>
18  */
19
20 #include <linux/module.h>
21 #include <linux/types.h>
22 #include <linux/kernel.h>
23 #include <linux/string.h>
24 #include <linux/errno.h>
25 #include <linux/skbuff.h>
26 #include <net/netlink.h>
27 #include <net/pkt_sched.h>
28
29
30 struct multiq_sched_data {
31         u16 bands;
32         u16 max_bands;
33         u16 curband;
34         struct tcf_proto *filter_list;
35         struct Qdisc **queues;
36 };
37
38
39 static struct Qdisc *
40 multiq_classify(struct sk_buff *skb, struct Qdisc *sch, int *qerr)
41 {
42         struct multiq_sched_data *q = qdisc_priv(sch);
43         u32 band;
44         struct tcf_result res;
45         int err;
46
47         *qerr = NET_XMIT_SUCCESS | __NET_XMIT_BYPASS;
48         err = tc_classify(skb, q->filter_list, &res);
49 #ifdef CONFIG_NET_CLS_ACT
50         switch (err) {
51         case TC_ACT_STOLEN:
52         case TC_ACT_QUEUED:
53                 *qerr = NET_XMIT_SUCCESS | __NET_XMIT_STOLEN;
54         case TC_ACT_SHOT:
55                 return NULL;
56         }
57 #endif
58         band = skb_get_queue_mapping(skb);
59
60         if (band >= q->bands)
61                 return q->queues[0];
62
63         return q->queues[band];
64 }
65
66 static int
67 multiq_enqueue(struct sk_buff *skb, struct Qdisc *sch)
68 {
69         struct Qdisc *qdisc;
70         int ret;
71
72         qdisc = multiq_classify(skb, sch, &ret);
73 #ifdef CONFIG_NET_CLS_ACT
74         if (qdisc == NULL) {
75
76                 if (ret & __NET_XMIT_BYPASS)
77                         sch->qstats.drops++;
78                 kfree_skb(skb);
79                 return ret;
80         }
81 #endif
82
83         ret = qdisc_enqueue(skb, qdisc);
84         if (ret == NET_XMIT_SUCCESS) {
85                 sch->bstats.bytes += qdisc_pkt_len(skb);
86                 sch->bstats.packets++;
87                 sch->q.qlen++;
88                 return NET_XMIT_SUCCESS;
89         }
90         if (net_xmit_drop_count(ret))
91                 sch->qstats.drops++;
92         return ret;
93 }
94
95
96 static int
97 multiq_requeue(struct sk_buff *skb, struct Qdisc *sch)
98 {
99         struct Qdisc *qdisc;
100         struct multiq_sched_data *q = qdisc_priv(sch);
101         int ret;
102
103         qdisc = multiq_classify(skb, sch, &ret);
104 #ifdef CONFIG_NET_CLS_ACT
105         if (qdisc == NULL) {
106                 if (ret & __NET_XMIT_BYPASS)
107                         sch->qstats.drops++;
108                 kfree_skb(skb);
109                 return ret;
110         }
111 #endif
112
113         ret = qdisc->ops->requeue(skb, qdisc);
114         if (ret == NET_XMIT_SUCCESS) {
115                 sch->q.qlen++;
116                 sch->qstats.requeues++;
117                 if (q->curband)
118                         q->curband--;
119                 else
120                         q->curband = q->bands - 1;
121                 return NET_XMIT_SUCCESS;
122         }
123         if (net_xmit_drop_count(ret))
124                 sch->qstats.drops++;
125         return ret;
126 }
127
128
129 static struct sk_buff *multiq_dequeue(struct Qdisc *sch)
130 {
131         struct multiq_sched_data *q = qdisc_priv(sch);
132         struct Qdisc *qdisc;
133         struct sk_buff *skb;
134         int band;
135
136         for (band = 0; band < q->bands; band++) {
137                 /* cycle through bands to ensure fairness */
138                 q->curband++;
139                 if (q->curband >= q->bands)
140                         q->curband = 0;
141
142                 /* Check that target subqueue is available before
143                  * pulling an skb to avoid excessive requeues
144                  */
145                 if (!__netif_subqueue_stopped(qdisc_dev(sch), q->curband)) {
146                         qdisc = q->queues[q->curband];
147                         skb = qdisc->dequeue(qdisc);
148                         if (skb) {
149                                 sch->q.qlen--;
150                                 return skb;
151                         }
152                 }
153         }
154         return NULL;
155
156 }
157
158 static struct sk_buff *multiq_peek(struct Qdisc *sch)
159 {
160         struct multiq_sched_data *q = qdisc_priv(sch);
161         unsigned int curband = q->curband;
162         struct Qdisc *qdisc;
163         struct sk_buff *skb;
164         int band;
165
166         for (band = 0; band < q->bands; band++) {
167                 /* cycle through bands to ensure fairness */
168                 curband++;
169                 if (curband >= q->bands)
170                         curband = 0;
171
172                 /* Check that target subqueue is available before
173                  * pulling an skb to avoid excessive requeues
174                  */
175                 if (!__netif_subqueue_stopped(qdisc_dev(sch), curband)) {
176                         qdisc = q->queues[curband];
177                         skb = qdisc->ops->peek(qdisc);
178                         if (skb)
179                                 return skb;
180                 }
181         }
182         return NULL;
183
184 }
185
186 static unsigned int multiq_drop(struct Qdisc *sch)
187 {
188         struct multiq_sched_data *q = qdisc_priv(sch);
189         int band;
190         unsigned int len;
191         struct Qdisc *qdisc;
192
193         for (band = q->bands-1; band >= 0; band--) {
194                 qdisc = q->queues[band];
195                 if (qdisc->ops->drop) {
196                         len = qdisc->ops->drop(qdisc);
197                         if (len != 0) {
198                                 sch->q.qlen--;
199                                 return len;
200                         }
201                 }
202         }
203         return 0;
204 }
205
206
207 static void
208 multiq_reset(struct Qdisc *sch)
209 {
210         u16 band;
211         struct multiq_sched_data *q = qdisc_priv(sch);
212
213         for (band = 0; band < q->bands; band++)
214                 qdisc_reset(q->queues[band]);
215         sch->q.qlen = 0;
216         q->curband = 0;
217 }
218
219 static void
220 multiq_destroy(struct Qdisc *sch)
221 {
222         int band;
223         struct multiq_sched_data *q = qdisc_priv(sch);
224
225         tcf_destroy_chain(&q->filter_list);
226         for (band = 0; band < q->bands; band++)
227                 qdisc_destroy(q->queues[band]);
228
229         kfree(q->queues);
230 }
231
232 static int multiq_tune(struct Qdisc *sch, struct nlattr *opt)
233 {
234         struct multiq_sched_data *q = qdisc_priv(sch);
235         struct tc_multiq_qopt *qopt;
236         int i;
237
238         if (!netif_is_multiqueue(qdisc_dev(sch)))
239                 return -EINVAL;
240         if (nla_len(opt) < sizeof(*qopt))
241                 return -EINVAL;
242
243         qopt = nla_data(opt);
244
245         qopt->bands = qdisc_dev(sch)->real_num_tx_queues;
246
247         sch_tree_lock(sch);
248         q->bands = qopt->bands;
249         for (i = q->bands; i < q->max_bands; i++) {
250                 if (q->queues[i] != &noop_qdisc) {
251                         struct Qdisc *child = xchg(&q->queues[i], &noop_qdisc);
252                         qdisc_tree_decrease_qlen(child, child->q.qlen);
253                         qdisc_destroy(child);
254                 }
255         }
256
257         sch_tree_unlock(sch);
258
259         for (i = 0; i < q->bands; i++) {
260                 if (q->queues[i] == &noop_qdisc) {
261                         struct Qdisc *child;
262                         child = qdisc_create_dflt(qdisc_dev(sch),
263                                                   sch->dev_queue,
264                                                   &pfifo_qdisc_ops,
265                                                   TC_H_MAKE(sch->handle,
266                                                             i + 1));
267                         if (child) {
268                                 sch_tree_lock(sch);
269                                 child = xchg(&q->queues[i], child);
270
271                                 if (child != &noop_qdisc) {
272                                         qdisc_tree_decrease_qlen(child,
273                                                                  child->q.qlen);
274                                         qdisc_destroy(child);
275                                 }
276                                 sch_tree_unlock(sch);
277                         }
278                 }
279         }
280         return 0;
281 }
282
283 static int multiq_init(struct Qdisc *sch, struct nlattr *opt)
284 {
285         struct multiq_sched_data *q = qdisc_priv(sch);
286         int i, err;
287
288         q->queues = NULL;
289
290         if (opt == NULL)
291                 return -EINVAL;
292
293         q->max_bands = qdisc_dev(sch)->num_tx_queues;
294
295         q->queues = kcalloc(q->max_bands, sizeof(struct Qdisc *), GFP_KERNEL);
296         if (!q->queues)
297                 return -ENOBUFS;
298         for (i = 0; i < q->max_bands; i++)
299                 q->queues[i] = &noop_qdisc;
300
301         err = multiq_tune(sch,opt);
302
303         if (err)
304                 kfree(q->queues);
305
306         return err;
307 }
308
309 static int multiq_dump(struct Qdisc *sch, struct sk_buff *skb)
310 {
311         struct multiq_sched_data *q = qdisc_priv(sch);
312         unsigned char *b = skb_tail_pointer(skb);
313         struct tc_multiq_qopt opt;
314
315         opt.bands = q->bands;
316         opt.max_bands = q->max_bands;
317
318         NLA_PUT(skb, TCA_OPTIONS, sizeof(opt), &opt);
319
320         return skb->len;
321
322 nla_put_failure:
323         nlmsg_trim(skb, b);
324         return -1;
325 }
326
327 static int multiq_graft(struct Qdisc *sch, unsigned long arg, struct Qdisc *new,
328                       struct Qdisc **old)
329 {
330         struct multiq_sched_data *q = qdisc_priv(sch);
331         unsigned long band = arg - 1;
332
333         if (band >= q->bands)
334                 return -EINVAL;
335
336         if (new == NULL)
337                 new = &noop_qdisc;
338
339         sch_tree_lock(sch);
340         *old = q->queues[band];
341         q->queues[band] = new;
342         qdisc_tree_decrease_qlen(*old, (*old)->q.qlen);
343         qdisc_reset(*old);
344         sch_tree_unlock(sch);
345
346         return 0;
347 }
348
349 static struct Qdisc *
350 multiq_leaf(struct Qdisc *sch, unsigned long arg)
351 {
352         struct multiq_sched_data *q = qdisc_priv(sch);
353         unsigned long band = arg - 1;
354
355         if (band >= q->bands)
356                 return NULL;
357
358         return q->queues[band];
359 }
360
361 static unsigned long multiq_get(struct Qdisc *sch, u32 classid)
362 {
363         struct multiq_sched_data *q = qdisc_priv(sch);
364         unsigned long band = TC_H_MIN(classid);
365
366         if (band - 1 >= q->bands)
367                 return 0;
368         return band;
369 }
370
371 static unsigned long multiq_bind(struct Qdisc *sch, unsigned long parent,
372                                  u32 classid)
373 {
374         return multiq_get(sch, classid);
375 }
376
377
378 static void multiq_put(struct Qdisc *q, unsigned long cl)
379 {
380         return;
381 }
382
383 static int multiq_change(struct Qdisc *sch, u32 handle, u32 parent,
384                          struct nlattr **tca, unsigned long *arg)
385 {
386         unsigned long cl = *arg;
387         struct multiq_sched_data *q = qdisc_priv(sch);
388
389         if (cl - 1 > q->bands)
390                 return -ENOENT;
391         return 0;
392 }
393
394 static int multiq_delete(struct Qdisc *sch, unsigned long cl)
395 {
396         struct multiq_sched_data *q = qdisc_priv(sch);
397         if (cl - 1 > q->bands)
398                 return -ENOENT;
399         return 0;
400 }
401
402
403 static int multiq_dump_class(struct Qdisc *sch, unsigned long cl,
404                              struct sk_buff *skb, struct tcmsg *tcm)
405 {
406         struct multiq_sched_data *q = qdisc_priv(sch);
407
408         if (cl - 1 > q->bands)
409                 return -ENOENT;
410         tcm->tcm_handle |= TC_H_MIN(cl);
411         if (q->queues[cl-1])
412                 tcm->tcm_info = q->queues[cl-1]->handle;
413         return 0;
414 }
415
416 static int multiq_dump_class_stats(struct Qdisc *sch, unsigned long cl,
417                                  struct gnet_dump *d)
418 {
419         struct multiq_sched_data *q = qdisc_priv(sch);
420         struct Qdisc *cl_q;
421
422         cl_q = q->queues[cl - 1];
423         if (gnet_stats_copy_basic(d, &cl_q->bstats) < 0 ||
424             gnet_stats_copy_queue(d, &cl_q->qstats) < 0)
425                 return -1;
426
427         return 0;
428 }
429
430 static void multiq_walk(struct Qdisc *sch, struct qdisc_walker *arg)
431 {
432         struct multiq_sched_data *q = qdisc_priv(sch);
433         int band;
434
435         if (arg->stop)
436                 return;
437
438         for (band = 0; band < q->bands; band++) {
439                 if (arg->count < arg->skip) {
440                         arg->count++;
441                         continue;
442                 }
443                 if (arg->fn(sch, band+1, arg) < 0) {
444                         arg->stop = 1;
445                         break;
446                 }
447                 arg->count++;
448         }
449 }
450
451 static struct tcf_proto **multiq_find_tcf(struct Qdisc *sch, unsigned long cl)
452 {
453         struct multiq_sched_data *q = qdisc_priv(sch);
454
455         if (cl)
456                 return NULL;
457         return &q->filter_list;
458 }
459
460 static const struct Qdisc_class_ops multiq_class_ops = {
461         .graft          =       multiq_graft,
462         .leaf           =       multiq_leaf,
463         .get            =       multiq_get,
464         .put            =       multiq_put,
465         .change         =       multiq_change,
466         .delete         =       multiq_delete,
467         .walk           =       multiq_walk,
468         .tcf_chain      =       multiq_find_tcf,
469         .bind_tcf       =       multiq_bind,
470         .unbind_tcf     =       multiq_put,
471         .dump           =       multiq_dump_class,
472         .dump_stats     =       multiq_dump_class_stats,
473 };
474
475 static struct Qdisc_ops multiq_qdisc_ops __read_mostly = {
476         .next           =       NULL,
477         .cl_ops         =       &multiq_class_ops,
478         .id             =       "multiq",
479         .priv_size      =       sizeof(struct multiq_sched_data),
480         .enqueue        =       multiq_enqueue,
481         .dequeue        =       multiq_dequeue,
482         .peek           =       multiq_peek,
483         .requeue        =       multiq_requeue,
484         .drop           =       multiq_drop,
485         .init           =       multiq_init,
486         .reset          =       multiq_reset,
487         .destroy        =       multiq_destroy,
488         .change         =       multiq_tune,
489         .dump           =       multiq_dump,
490         .owner          =       THIS_MODULE,
491 };
492
493 static int __init multiq_module_init(void)
494 {
495         return register_qdisc(&multiq_qdisc_ops);
496 }
497
498 static void __exit multiq_module_exit(void)
499 {
500         unregister_qdisc(&multiq_qdisc_ops);
501 }
502
503 module_init(multiq_module_init)
504 module_exit(multiq_module_exit)
505
506 MODULE_LICENSE("GPL");