1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19#include <linux/module.h>
20#include <linux/slab.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#include <net/pkt_cls.h>
29
30struct multiq_sched_data {
31 u16 bands;
32 u16 max_bands;
33 u16 curband;
34 struct tcf_proto __rcu *filter_list;
35 struct tcf_block *block;
36 struct Qdisc **queues;
37};
38
39
40static struct Qdisc *
41multiq_classify(struct sk_buff *skb, struct Qdisc *sch, int *qerr)
42{
43 struct multiq_sched_data *q = qdisc_priv(sch);
44 u32 band;
45 struct tcf_result res;
46 struct tcf_proto *fl = rcu_dereference_bh(q->filter_list);
47 int err;
48
49 *qerr = NET_XMIT_SUCCESS | __NET_XMIT_BYPASS;
50 err = tcf_classify(skb, fl, &res, false);
51#ifdef CONFIG_NET_CLS_ACT
52 switch (err) {
53 case TC_ACT_STOLEN:
54 case TC_ACT_QUEUED:
55 case TC_ACT_TRAP:
56 *qerr = NET_XMIT_SUCCESS | __NET_XMIT_STOLEN;
57
58 case TC_ACT_SHOT:
59 return NULL;
60 }
61#endif
62 band = skb_get_queue_mapping(skb);
63
64 if (band >= q->bands)
65 return q->queues[0];
66
67 return q->queues[band];
68}
69
70static int
71multiq_enqueue(struct sk_buff *skb, struct Qdisc *sch,
72 struct sk_buff **to_free)
73{
74 struct Qdisc *qdisc;
75 int ret;
76
77 qdisc = multiq_classify(skb, sch, &ret);
78#ifdef CONFIG_NET_CLS_ACT
79 if (qdisc == NULL) {
80
81 if (ret & __NET_XMIT_BYPASS)
82 qdisc_qstats_drop(sch);
83 __qdisc_drop(skb, to_free);
84 return ret;
85 }
86#endif
87
88 ret = qdisc_enqueue(skb, qdisc, to_free);
89 if (ret == NET_XMIT_SUCCESS) {
90 sch->q.qlen++;
91 return NET_XMIT_SUCCESS;
92 }
93 if (net_xmit_drop_count(ret))
94 qdisc_qstats_drop(sch);
95 return ret;
96}
97
98static struct sk_buff *multiq_dequeue(struct Qdisc *sch)
99{
100 struct multiq_sched_data *q = qdisc_priv(sch);
101 struct Qdisc *qdisc;
102 struct sk_buff *skb;
103 int band;
104
105 for (band = 0; band < q->bands; band++) {
106
107 q->curband++;
108 if (q->curband >= q->bands)
109 q->curband = 0;
110
111
112
113
114 if (!netif_xmit_stopped(
115 netdev_get_tx_queue(qdisc_dev(sch), q->curband))) {
116 qdisc = q->queues[q->curband];
117 skb = qdisc->dequeue(qdisc);
118 if (skb) {
119 qdisc_bstats_update(sch, skb);
120 sch->q.qlen--;
121 return skb;
122 }
123 }
124 }
125 return NULL;
126
127}
128
129static struct sk_buff *multiq_peek(struct Qdisc *sch)
130{
131 struct multiq_sched_data *q = qdisc_priv(sch);
132 unsigned int curband = q->curband;
133 struct Qdisc *qdisc;
134 struct sk_buff *skb;
135 int band;
136
137 for (band = 0; band < q->bands; band++) {
138
139 curband++;
140 if (curband >= q->bands)
141 curband = 0;
142
143
144
145
146 if (!netif_xmit_stopped(
147 netdev_get_tx_queue(qdisc_dev(sch), curband))) {
148 qdisc = q->queues[curband];
149 skb = qdisc->ops->peek(qdisc);
150 if (skb)
151 return skb;
152 }
153 }
154 return NULL;
155
156}
157
158static void
159multiq_reset(struct Qdisc *sch)
160{
161 u16 band;
162 struct multiq_sched_data *q = qdisc_priv(sch);
163
164 for (band = 0; band < q->bands; band++)
165 qdisc_reset(q->queues[band]);
166 sch->q.qlen = 0;
167 q->curband = 0;
168}
169
170static void
171multiq_destroy(struct Qdisc *sch)
172{
173 int band;
174 struct multiq_sched_data *q = qdisc_priv(sch);
175
176 tcf_block_put(q->block);
177 for (band = 0; band < q->bands; band++)
178 qdisc_put(q->queues[band]);
179
180 kfree(q->queues);
181}
182
183static int multiq_tune(struct Qdisc *sch, struct nlattr *opt,
184 struct netlink_ext_ack *extack)
185{
186 struct multiq_sched_data *q = qdisc_priv(sch);
187 struct tc_multiq_qopt *qopt;
188 struct Qdisc **removed;
189 int i, n_removed = 0;
190
191 if (!netif_is_multiqueue(qdisc_dev(sch)))
192 return -EOPNOTSUPP;
193 if (nla_len(opt) < sizeof(*qopt))
194 return -EINVAL;
195
196 qopt = nla_data(opt);
197
198 qopt->bands = qdisc_dev(sch)->real_num_tx_queues;
199
200 removed = kmalloc(sizeof(*removed) * (q->max_bands - q->bands),
201 GFP_KERNEL);
202 if (!removed)
203 return -ENOMEM;
204
205 sch_tree_lock(sch);
206 q->bands = qopt->bands;
207 for (i = q->bands; i < q->max_bands; i++) {
208 if (q->queues[i] != &noop_qdisc) {
209 struct Qdisc *child = q->queues[i];
210
211 q->queues[i] = &noop_qdisc;
212 qdisc_purge_queue(child);
213 removed[n_removed++] = child;
214 }
215 }
216
217 sch_tree_unlock(sch);
218
219 for (i = 0; i < n_removed; i++)
220 qdisc_put(removed[i]);
221 kfree(removed);
222
223 for (i = 0; i < q->bands; i++) {
224 if (q->queues[i] == &noop_qdisc) {
225 struct Qdisc *child, *old;
226 child = qdisc_create_dflt(sch->dev_queue,
227 &pfifo_qdisc_ops,
228 TC_H_MAKE(sch->handle,
229 i + 1), extack);
230 if (child) {
231 sch_tree_lock(sch);
232 old = q->queues[i];
233 q->queues[i] = child;
234 if (child != &noop_qdisc)
235 qdisc_hash_add(child, true);
236
237 if (old != &noop_qdisc)
238 qdisc_purge_queue(old);
239 sch_tree_unlock(sch);
240 qdisc_put(old);
241 }
242 }
243 }
244 return 0;
245}
246
247static int multiq_init(struct Qdisc *sch, struct nlattr *opt,
248 struct netlink_ext_ack *extack)
249{
250 struct multiq_sched_data *q = qdisc_priv(sch);
251 int i, err;
252
253 q->queues = NULL;
254
255 if (!opt)
256 return -EINVAL;
257
258 err = tcf_block_get(&q->block, &q->filter_list, sch, extack);
259 if (err)
260 return err;
261
262 q->max_bands = qdisc_dev(sch)->num_tx_queues;
263
264 q->queues = kcalloc(q->max_bands, sizeof(struct Qdisc *), GFP_KERNEL);
265 if (!q->queues)
266 return -ENOBUFS;
267 for (i = 0; i < q->max_bands; i++)
268 q->queues[i] = &noop_qdisc;
269
270 return multiq_tune(sch, opt, extack);
271}
272
273static int multiq_dump(struct Qdisc *sch, struct sk_buff *skb)
274{
275 struct multiq_sched_data *q = qdisc_priv(sch);
276 unsigned char *b = skb_tail_pointer(skb);
277 struct tc_multiq_qopt opt;
278
279 opt.bands = q->bands;
280 opt.max_bands = q->max_bands;
281
282 if (nla_put(skb, TCA_OPTIONS, sizeof(opt), &opt))
283 goto nla_put_failure;
284
285 return skb->len;
286
287nla_put_failure:
288 nlmsg_trim(skb, b);
289 return -1;
290}
291
292static int multiq_graft(struct Qdisc *sch, unsigned long arg, struct Qdisc *new,
293 struct Qdisc **old, struct netlink_ext_ack *extack)
294{
295 struct multiq_sched_data *q = qdisc_priv(sch);
296 unsigned long band = arg - 1;
297
298 if (new == NULL)
299 new = &noop_qdisc;
300
301 *old = qdisc_replace(sch, new, &q->queues[band]);
302 return 0;
303}
304
305static struct Qdisc *
306multiq_leaf(struct Qdisc *sch, unsigned long arg)
307{
308 struct multiq_sched_data *q = qdisc_priv(sch);
309 unsigned long band = arg - 1;
310
311 return q->queues[band];
312}
313
314static unsigned long multiq_find(struct Qdisc *sch, u32 classid)
315{
316 struct multiq_sched_data *q = qdisc_priv(sch);
317 unsigned long band = TC_H_MIN(classid);
318
319 if (band - 1 >= q->bands)
320 return 0;
321 return band;
322}
323
324static unsigned long multiq_bind(struct Qdisc *sch, unsigned long parent,
325 u32 classid)
326{
327 return multiq_find(sch, classid);
328}
329
330
331static void multiq_unbind(struct Qdisc *q, unsigned long cl)
332{
333}
334
335static int multiq_dump_class(struct Qdisc *sch, unsigned long cl,
336 struct sk_buff *skb, struct tcmsg *tcm)
337{
338 struct multiq_sched_data *q = qdisc_priv(sch);
339
340 tcm->tcm_handle |= TC_H_MIN(cl);
341 tcm->tcm_info = q->queues[cl - 1]->handle;
342 return 0;
343}
344
345static int multiq_dump_class_stats(struct Qdisc *sch, unsigned long cl,
346 struct gnet_dump *d)
347{
348 struct multiq_sched_data *q = qdisc_priv(sch);
349 struct Qdisc *cl_q;
350
351 cl_q = q->queues[cl - 1];
352 if (gnet_stats_copy_basic(qdisc_root_sleeping_running(sch),
353 d, cl_q->cpu_bstats, &cl_q->bstats) < 0 ||
354 qdisc_qstats_copy(d, cl_q) < 0)
355 return -1;
356
357 return 0;
358}
359
360static void multiq_walk(struct Qdisc *sch, struct qdisc_walker *arg)
361{
362 struct multiq_sched_data *q = qdisc_priv(sch);
363 int band;
364
365 if (arg->stop)
366 return;
367
368 for (band = 0; band < q->bands; band++) {
369 if (arg->count < arg->skip) {
370 arg->count++;
371 continue;
372 }
373 if (arg->fn(sch, band + 1, arg) < 0) {
374 arg->stop = 1;
375 break;
376 }
377 arg->count++;
378 }
379}
380
381static struct tcf_block *multiq_tcf_block(struct Qdisc *sch, unsigned long cl,
382 struct netlink_ext_ack *extack)
383{
384 struct multiq_sched_data *q = qdisc_priv(sch);
385
386 if (cl)
387 return NULL;
388 return q->block;
389}
390
391static const struct Qdisc_class_ops multiq_class_ops = {
392 .graft = multiq_graft,
393 .leaf = multiq_leaf,
394 .find = multiq_find,
395 .walk = multiq_walk,
396 .tcf_block = multiq_tcf_block,
397 .bind_tcf = multiq_bind,
398 .unbind_tcf = multiq_unbind,
399 .dump = multiq_dump_class,
400 .dump_stats = multiq_dump_class_stats,
401};
402
403static struct Qdisc_ops multiq_qdisc_ops __read_mostly = {
404 .next = NULL,
405 .cl_ops = &multiq_class_ops,
406 .id = "multiq",
407 .priv_size = sizeof(struct multiq_sched_data),
408 .enqueue = multiq_enqueue,
409 .dequeue = multiq_dequeue,
410 .peek = multiq_peek,
411 .init = multiq_init,
412 .reset = multiq_reset,
413 .destroy = multiq_destroy,
414 .change = multiq_tune,
415 .dump = multiq_dump,
416 .owner = THIS_MODULE,
417};
418
419static int __init multiq_module_init(void)
420{
421 return register_qdisc(&multiq_qdisc_ops);
422}
423
424static void __exit multiq_module_exit(void)
425{
426 unregister_qdisc(&multiq_qdisc_ops);
427}
428
429module_init(multiq_module_init)
430module_exit(multiq_module_exit)
431
432MODULE_LICENSE("GPL");
433
