xref: /openbmc/linux/net/sched/sch_multiq.c (revision ae0be8de) 
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, see <http://www.gnu.org/licenses/>.
15  *
16  * Author: Alexander Duyck <alexander.h.duyck@intel.com>
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 
30 struct 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 
40 static struct Qdisc *
41 multiq_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 		/* fall through */
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 
70 static int
71 multiq_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 
98 static 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 		/* cycle through bands to ensure fairness */
107 		q->curband++;
108 		if (q->curband >= q->bands)
109 			q->curband = 0;
110 
111 		/* Check that target subqueue is available before
112 		 * pulling an skb to avoid head-of-line blocking.
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 
129 static 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 		/* cycle through bands to ensure fairness */
139 		curband++;
140 		if (curband >= q->bands)
141 			curband = 0;
142 
143 		/* Check that target subqueue is available before
144 		 * pulling an skb to avoid head-of-line blocking.
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 
158 static void
159 multiq_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 
170 static void
171 multiq_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 
183 static 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 	int i;
189 
190 	if (!netif_is_multiqueue(qdisc_dev(sch)))
191 		return -EOPNOTSUPP;
192 	if (nla_len(opt) < sizeof(*qopt))
193 		return -EINVAL;
194 
195 	qopt = nla_data(opt);
196 
197 	qopt->bands = qdisc_dev(sch)->real_num_tx_queues;
198 
199 	sch_tree_lock(sch);
200 	q->bands = qopt->bands;
201 	for (i = q->bands; i < q->max_bands; i++) {
202 		if (q->queues[i] != &noop_qdisc) {
203 			struct Qdisc *child = q->queues[i];
204 
205 			q->queues[i] = &noop_qdisc;
206 			qdisc_tree_flush_backlog(child);
207 			qdisc_put(child);
208 		}
209 	}
210 
211 	sch_tree_unlock(sch);
212 
213 	for (i = 0; i < q->bands; i++) {
214 		if (q->queues[i] == &noop_qdisc) {
215 			struct Qdisc *child, *old;
216 			child = qdisc_create_dflt(sch->dev_queue,
217 						  &pfifo_qdisc_ops,
218 						  TC_H_MAKE(sch->handle,
219 							    i + 1), extack);
220 			if (child) {
221 				sch_tree_lock(sch);
222 				old = q->queues[i];
223 				q->queues[i] = child;
224 				if (child != &noop_qdisc)
225 					qdisc_hash_add(child, true);
226 
227 				if (old != &noop_qdisc) {
228 					qdisc_tree_flush_backlog(old);
229 					qdisc_put(old);
230 				}
231 				sch_tree_unlock(sch);
232 			}
233 		}
234 	}
235 	return 0;
236 }
237 
238 static int multiq_init(struct Qdisc *sch, struct nlattr *opt,
239 		       struct netlink_ext_ack *extack)
240 {
241 	struct multiq_sched_data *q = qdisc_priv(sch);
242 	int i, err;
243 
244 	q->queues = NULL;
245 
246 	if (!opt)
247 		return -EINVAL;
248 
249 	err = tcf_block_get(&q->block, &q->filter_list, sch, extack);
250 	if (err)
251 		return err;
252 
253 	q->max_bands = qdisc_dev(sch)->num_tx_queues;
254 
255 	q->queues = kcalloc(q->max_bands, sizeof(struct Qdisc *), GFP_KERNEL);
256 	if (!q->queues)
257 		return -ENOBUFS;
258 	for (i = 0; i < q->max_bands; i++)
259 		q->queues[i] = &noop_qdisc;
260 
261 	return multiq_tune(sch, opt, extack);
262 }
263 
264 static int multiq_dump(struct Qdisc *sch, struct sk_buff *skb)
265 {
266 	struct multiq_sched_data *q = qdisc_priv(sch);
267 	unsigned char *b = skb_tail_pointer(skb);
268 	struct tc_multiq_qopt opt;
269 
270 	opt.bands = q->bands;
271 	opt.max_bands = q->max_bands;
272 
273 	if (nla_put(skb, TCA_OPTIONS, sizeof(opt), &opt))
274 		goto nla_put_failure;
275 
276 	return skb->len;
277 
278 nla_put_failure:
279 	nlmsg_trim(skb, b);
280 	return -1;
281 }
282 
283 static int multiq_graft(struct Qdisc *sch, unsigned long arg, struct Qdisc *new,
284 			struct Qdisc **old, struct netlink_ext_ack *extack)
285 {
286 	struct multiq_sched_data *q = qdisc_priv(sch);
287 	unsigned long band = arg - 1;
288 
289 	if (new == NULL)
290 		new = &noop_qdisc;
291 
292 	*old = qdisc_replace(sch, new, &q->queues[band]);
293 	return 0;
294 }
295 
296 static struct Qdisc *
297 multiq_leaf(struct Qdisc *sch, unsigned long arg)
298 {
299 	struct multiq_sched_data *q = qdisc_priv(sch);
300 	unsigned long band = arg - 1;
301 
302 	return q->queues[band];
303 }
304 
305 static unsigned long multiq_find(struct Qdisc *sch, u32 classid)
306 {
307 	struct multiq_sched_data *q = qdisc_priv(sch);
308 	unsigned long band = TC_H_MIN(classid);
309 
310 	if (band - 1 >= q->bands)
311 		return 0;
312 	return band;
313 }
314 
315 static unsigned long multiq_bind(struct Qdisc *sch, unsigned long parent,
316 				 u32 classid)
317 {
318 	return multiq_find(sch, classid);
319 }
320 
321 
322 static void multiq_unbind(struct Qdisc *q, unsigned long cl)
323 {
324 }
325 
326 static int multiq_dump_class(struct Qdisc *sch, unsigned long cl,
327 			     struct sk_buff *skb, struct tcmsg *tcm)
328 {
329 	struct multiq_sched_data *q = qdisc_priv(sch);
330 
331 	tcm->tcm_handle |= TC_H_MIN(cl);
332 	tcm->tcm_info = q->queues[cl - 1]->handle;
333 	return 0;
334 }
335 
336 static int multiq_dump_class_stats(struct Qdisc *sch, unsigned long cl,
337 				 struct gnet_dump *d)
338 {
339 	struct multiq_sched_data *q = qdisc_priv(sch);
340 	struct Qdisc *cl_q;
341 
342 	cl_q = q->queues[cl - 1];
343 	if (gnet_stats_copy_basic(qdisc_root_sleeping_running(sch),
344 				  d, NULL, &cl_q->bstats) < 0 ||
345 	    qdisc_qstats_copy(d, cl_q) < 0)
346 		return -1;
347 
348 	return 0;
349 }
350 
351 static void multiq_walk(struct Qdisc *sch, struct qdisc_walker *arg)
352 {
353 	struct multiq_sched_data *q = qdisc_priv(sch);
354 	int band;
355 
356 	if (arg->stop)
357 		return;
358 
359 	for (band = 0; band < q->bands; band++) {
360 		if (arg->count < arg->skip) {
361 			arg->count++;
362 			continue;
363 		}
364 		if (arg->fn(sch, band + 1, arg) < 0) {
365 			arg->stop = 1;
366 			break;
367 		}
368 		arg->count++;
369 	}
370 }
371 
372 static struct tcf_block *multiq_tcf_block(struct Qdisc *sch, unsigned long cl,
373 					  struct netlink_ext_ack *extack)
374 {
375 	struct multiq_sched_data *q = qdisc_priv(sch);
376 
377 	if (cl)
378 		return NULL;
379 	return q->block;
380 }
381 
382 static const struct Qdisc_class_ops multiq_class_ops = {
383 	.graft		=	multiq_graft,
384 	.leaf		=	multiq_leaf,
385 	.find		=	multiq_find,
386 	.walk		=	multiq_walk,
387 	.tcf_block	=	multiq_tcf_block,
388 	.bind_tcf	=	multiq_bind,
389 	.unbind_tcf	=	multiq_unbind,
390 	.dump		=	multiq_dump_class,
391 	.dump_stats	=	multiq_dump_class_stats,
392 };
393 
394 static struct Qdisc_ops multiq_qdisc_ops __read_mostly = {
395 	.next		=	NULL,
396 	.cl_ops		=	&multiq_class_ops,
397 	.id		=	"multiq",
398 	.priv_size	=	sizeof(struct multiq_sched_data),
399 	.enqueue	=	multiq_enqueue,
400 	.dequeue	=	multiq_dequeue,
401 	.peek		=	multiq_peek,
402 	.init		=	multiq_init,
403 	.reset		=	multiq_reset,
404 	.destroy	=	multiq_destroy,
405 	.change		=	multiq_tune,
406 	.dump		=	multiq_dump,
407 	.owner		=	THIS_MODULE,
408 };
409 
410 static int __init multiq_module_init(void)
411 {
412 	return register_qdisc(&multiq_qdisc_ops);
413 }
414 
415 static void __exit multiq_module_exit(void)
416 {
417 	unregister_qdisc(&multiq_qdisc_ops);
418 }
419 
420 module_init(multiq_module_init)
421 module_exit(multiq_module_exit)
422 
423 MODULE_LICENSE("GPL");
424