xref: /openbmc/linux/net/sched/sch_mqprio.c (revision 6c58c881)
1 // SPDX-License-Identifier: GPL-2.0-only
2 /*
3  * net/sched/sch_mqprio.c
4  *
5  * Copyright (c) 2010 John Fastabend <john.r.fastabend@intel.com>
6  */
7 
8 #include <linux/ethtool_netlink.h>
9 #include <linux/types.h>
10 #include <linux/slab.h>
11 #include <linux/kernel.h>
12 #include <linux/string.h>
13 #include <linux/errno.h>
14 #include <linux/skbuff.h>
15 #include <linux/module.h>
16 #include <net/netlink.h>
17 #include <net/pkt_sched.h>
18 #include <net/sch_generic.h>
19 #include <net/pkt_cls.h>
20 
21 #include "sch_mqprio_lib.h"
22 
23 struct mqprio_sched {
24 	struct Qdisc		**qdiscs;
25 	u16 mode;
26 	u16 shaper;
27 	int hw_offload;
28 	u32 flags;
29 	u64 min_rate[TC_QOPT_MAX_QUEUE];
30 	u64 max_rate[TC_QOPT_MAX_QUEUE];
31 	u32 fp[TC_QOPT_MAX_QUEUE];
32 };
33 
mqprio_enable_offload(struct Qdisc * sch,const struct tc_mqprio_qopt * qopt,struct netlink_ext_ack * extack)34 static int mqprio_enable_offload(struct Qdisc *sch,
35 				 const struct tc_mqprio_qopt *qopt,
36 				 struct netlink_ext_ack *extack)
37 {
38 	struct mqprio_sched *priv = qdisc_priv(sch);
39 	struct net_device *dev = qdisc_dev(sch);
40 	struct tc_mqprio_qopt_offload mqprio = {
41 		.qopt = *qopt,
42 		.extack = extack,
43 	};
44 	int err, i;
45 
46 	switch (priv->mode) {
47 	case TC_MQPRIO_MODE_DCB:
48 		if (priv->shaper != TC_MQPRIO_SHAPER_DCB)
49 			return -EINVAL;
50 		break;
51 	case TC_MQPRIO_MODE_CHANNEL:
52 		mqprio.flags = priv->flags;
53 		if (priv->flags & TC_MQPRIO_F_MODE)
54 			mqprio.mode = priv->mode;
55 		if (priv->flags & TC_MQPRIO_F_SHAPER)
56 			mqprio.shaper = priv->shaper;
57 		if (priv->flags & TC_MQPRIO_F_MIN_RATE)
58 			for (i = 0; i < mqprio.qopt.num_tc; i++)
59 				mqprio.min_rate[i] = priv->min_rate[i];
60 		if (priv->flags & TC_MQPRIO_F_MAX_RATE)
61 			for (i = 0; i < mqprio.qopt.num_tc; i++)
62 				mqprio.max_rate[i] = priv->max_rate[i];
63 		break;
64 	default:
65 		return -EINVAL;
66 	}
67 
68 	mqprio_fp_to_offload(priv->fp, &mqprio);
69 
70 	err = dev->netdev_ops->ndo_setup_tc(dev, TC_SETUP_QDISC_MQPRIO,
71 					    &mqprio);
72 	if (err)
73 		return err;
74 
75 	priv->hw_offload = mqprio.qopt.hw;
76 
77 	return 0;
78 }
79 
mqprio_disable_offload(struct Qdisc * sch)80 static void mqprio_disable_offload(struct Qdisc *sch)
81 {
82 	struct tc_mqprio_qopt_offload mqprio = { { 0 } };
83 	struct mqprio_sched *priv = qdisc_priv(sch);
84 	struct net_device *dev = qdisc_dev(sch);
85 
86 	switch (priv->mode) {
87 	case TC_MQPRIO_MODE_DCB:
88 	case TC_MQPRIO_MODE_CHANNEL:
89 		dev->netdev_ops->ndo_setup_tc(dev, TC_SETUP_QDISC_MQPRIO,
90 					      &mqprio);
91 		break;
92 	}
93 }
94 
mqprio_destroy(struct Qdisc * sch)95 static void mqprio_destroy(struct Qdisc *sch)
96 {
97 	struct net_device *dev = qdisc_dev(sch);
98 	struct mqprio_sched *priv = qdisc_priv(sch);
99 	unsigned int ntx;
100 
101 	if (priv->qdiscs) {
102 		for (ntx = 0;
103 		     ntx < dev->num_tx_queues && priv->qdiscs[ntx];
104 		     ntx++)
105 			qdisc_put(priv->qdiscs[ntx]);
106 		kfree(priv->qdiscs);
107 	}
108 
109 	if (priv->hw_offload && dev->netdev_ops->ndo_setup_tc)
110 		mqprio_disable_offload(sch);
111 	else
112 		netdev_set_num_tc(dev, 0);
113 }
114 
mqprio_parse_opt(struct net_device * dev,struct tc_mqprio_qopt * qopt,const struct tc_mqprio_caps * caps,struct netlink_ext_ack * extack)115 static int mqprio_parse_opt(struct net_device *dev, struct tc_mqprio_qopt *qopt,
116 			    const struct tc_mqprio_caps *caps,
117 			    struct netlink_ext_ack *extack)
118 {
119 	int err;
120 
121 	/* Limit qopt->hw to maximum supported offload value.  Drivers have
122 	 * the option of overriding this later if they don't support the a
123 	 * given offload type.
124 	 */
125 	if (qopt->hw > TC_MQPRIO_HW_OFFLOAD_MAX)
126 		qopt->hw = TC_MQPRIO_HW_OFFLOAD_MAX;
127 
128 	/* If hardware offload is requested, we will leave 3 options to the
129 	 * device driver:
130 	 * - populate the queue counts itself (and ignore what was requested)
131 	 * - validate the provided queue counts by itself (and apply them)
132 	 * - request queue count validation here (and apply them)
133 	 */
134 	err = mqprio_validate_qopt(dev, qopt,
135 				   !qopt->hw || caps->validate_queue_counts,
136 				   false, extack);
137 	if (err)
138 		return err;
139 
140 	/* If ndo_setup_tc is not present then hardware doesn't support offload
141 	 * and we should return an error.
142 	 */
143 	if (qopt->hw && !dev->netdev_ops->ndo_setup_tc) {
144 		NL_SET_ERR_MSG(extack,
145 			       "Device does not support hardware offload");
146 		return -EINVAL;
147 	}
148 
149 	return 0;
150 }
151 
152 static const struct
153 nla_policy mqprio_tc_entry_policy[TCA_MQPRIO_TC_ENTRY_MAX + 1] = {
154 	[TCA_MQPRIO_TC_ENTRY_INDEX]	= NLA_POLICY_MAX(NLA_U32,
155 							 TC_QOPT_MAX_QUEUE),
156 	[TCA_MQPRIO_TC_ENTRY_FP]	= NLA_POLICY_RANGE(NLA_U32,
157 							   TC_FP_EXPRESS,
158 							   TC_FP_PREEMPTIBLE),
159 };
160 
161 static const struct nla_policy mqprio_policy[TCA_MQPRIO_MAX + 1] = {
162 	[TCA_MQPRIO_MODE]	= { .len = sizeof(u16) },
163 	[TCA_MQPRIO_SHAPER]	= { .len = sizeof(u16) },
164 	[TCA_MQPRIO_MIN_RATE64]	= { .type = NLA_NESTED },
165 	[TCA_MQPRIO_MAX_RATE64]	= { .type = NLA_NESTED },
166 	[TCA_MQPRIO_TC_ENTRY]	= { .type = NLA_NESTED },
167 };
168 
mqprio_parse_tc_entry(u32 fp[TC_QOPT_MAX_QUEUE],struct nlattr * opt,unsigned long * seen_tcs,struct netlink_ext_ack * extack)169 static int mqprio_parse_tc_entry(u32 fp[TC_QOPT_MAX_QUEUE],
170 				 struct nlattr *opt,
171 				 unsigned long *seen_tcs,
172 				 struct netlink_ext_ack *extack)
173 {
174 	struct nlattr *tb[TCA_MQPRIO_TC_ENTRY_MAX + 1];
175 	int err, tc;
176 
177 	err = nla_parse_nested(tb, TCA_MQPRIO_TC_ENTRY_MAX, opt,
178 			       mqprio_tc_entry_policy, extack);
179 	if (err < 0)
180 		return err;
181 
182 	if (NL_REQ_ATTR_CHECK(extack, opt, tb, TCA_MQPRIO_TC_ENTRY_INDEX)) {
183 		NL_SET_ERR_MSG(extack, "TC entry index missing");
184 		return -EINVAL;
185 	}
186 
187 	tc = nla_get_u32(tb[TCA_MQPRIO_TC_ENTRY_INDEX]);
188 	if (*seen_tcs & BIT(tc)) {
189 		NL_SET_ERR_MSG_ATTR(extack, tb[TCA_MQPRIO_TC_ENTRY_INDEX],
190 				    "Duplicate tc entry");
191 		return -EINVAL;
192 	}
193 
194 	*seen_tcs |= BIT(tc);
195 
196 	if (tb[TCA_MQPRIO_TC_ENTRY_FP])
197 		fp[tc] = nla_get_u32(tb[TCA_MQPRIO_TC_ENTRY_FP]);
198 
199 	return 0;
200 }
201 
mqprio_parse_tc_entries(struct Qdisc * sch,struct nlattr * nlattr_opt,int nlattr_opt_len,struct netlink_ext_ack * extack)202 static int mqprio_parse_tc_entries(struct Qdisc *sch, struct nlattr *nlattr_opt,
203 				   int nlattr_opt_len,
204 				   struct netlink_ext_ack *extack)
205 {
206 	struct mqprio_sched *priv = qdisc_priv(sch);
207 	struct net_device *dev = qdisc_dev(sch);
208 	bool have_preemption = false;
209 	unsigned long seen_tcs = 0;
210 	u32 fp[TC_QOPT_MAX_QUEUE];
211 	struct nlattr *n;
212 	int tc, rem;
213 	int err = 0;
214 
215 	for (tc = 0; tc < TC_QOPT_MAX_QUEUE; tc++)
216 		fp[tc] = priv->fp[tc];
217 
218 	nla_for_each_attr(n, nlattr_opt, nlattr_opt_len, rem) {
219 		if (nla_type(n) != TCA_MQPRIO_TC_ENTRY)
220 			continue;
221 
222 		err = mqprio_parse_tc_entry(fp, n, &seen_tcs, extack);
223 		if (err)
224 			goto out;
225 	}
226 
227 	for (tc = 0; tc < TC_QOPT_MAX_QUEUE; tc++) {
228 		priv->fp[tc] = fp[tc];
229 		if (fp[tc] == TC_FP_PREEMPTIBLE)
230 			have_preemption = true;
231 	}
232 
233 	if (have_preemption && !ethtool_dev_mm_supported(dev)) {
234 		NL_SET_ERR_MSG(extack, "Device does not support preemption");
235 		return -EOPNOTSUPP;
236 	}
237 out:
238 	return err;
239 }
240 
241 /* Parse the other netlink attributes that represent the payload of
242  * TCA_OPTIONS, which are appended right after struct tc_mqprio_qopt.
243  */
mqprio_parse_nlattr(struct Qdisc * sch,struct tc_mqprio_qopt * qopt,struct nlattr * opt,struct netlink_ext_ack * extack)244 static int mqprio_parse_nlattr(struct Qdisc *sch, struct tc_mqprio_qopt *qopt,
245 			       struct nlattr *opt,
246 			       struct netlink_ext_ack *extack)
247 {
248 	struct nlattr *nlattr_opt = nla_data(opt) + NLA_ALIGN(sizeof(*qopt));
249 	int nlattr_opt_len = nla_len(opt) - NLA_ALIGN(sizeof(*qopt));
250 	struct mqprio_sched *priv = qdisc_priv(sch);
251 	struct nlattr *tb[TCA_MQPRIO_MAX + 1] = {};
252 	struct nlattr *attr;
253 	int i, rem, err;
254 
255 	if (nlattr_opt_len >= nla_attr_size(0)) {
256 		err = nla_parse_deprecated(tb, TCA_MQPRIO_MAX, nlattr_opt,
257 					   nlattr_opt_len, mqprio_policy,
258 					   NULL);
259 		if (err < 0)
260 			return err;
261 	}
262 
263 	if (!qopt->hw) {
264 		NL_SET_ERR_MSG(extack,
265 			       "mqprio TCA_OPTIONS can only contain netlink attributes in hardware mode");
266 		return -EINVAL;
267 	}
268 
269 	if (tb[TCA_MQPRIO_MODE]) {
270 		priv->flags |= TC_MQPRIO_F_MODE;
271 		priv->mode = nla_get_u16(tb[TCA_MQPRIO_MODE]);
272 	}
273 
274 	if (tb[TCA_MQPRIO_SHAPER]) {
275 		priv->flags |= TC_MQPRIO_F_SHAPER;
276 		priv->shaper = nla_get_u16(tb[TCA_MQPRIO_SHAPER]);
277 	}
278 
279 	if (tb[TCA_MQPRIO_MIN_RATE64]) {
280 		if (priv->shaper != TC_MQPRIO_SHAPER_BW_RATE) {
281 			NL_SET_ERR_MSG_ATTR(extack, tb[TCA_MQPRIO_MIN_RATE64],
282 					    "min_rate accepted only when shaper is in bw_rlimit mode");
283 			return -EINVAL;
284 		}
285 		i = 0;
286 		nla_for_each_nested(attr, tb[TCA_MQPRIO_MIN_RATE64],
287 				    rem) {
288 			if (nla_type(attr) != TCA_MQPRIO_MIN_RATE64) {
289 				NL_SET_ERR_MSG_ATTR(extack, attr,
290 						    "Attribute type expected to be TCA_MQPRIO_MIN_RATE64");
291 				return -EINVAL;
292 			}
293 
294 			if (nla_len(attr) != sizeof(u64)) {
295 				NL_SET_ERR_MSG_ATTR(extack, attr,
296 						    "Attribute TCA_MQPRIO_MIN_RATE64 expected to have 8 bytes length");
297 				return -EINVAL;
298 			}
299 
300 			if (i >= qopt->num_tc)
301 				break;
302 			priv->min_rate[i] = nla_get_u64(attr);
303 			i++;
304 		}
305 		priv->flags |= TC_MQPRIO_F_MIN_RATE;
306 	}
307 
308 	if (tb[TCA_MQPRIO_MAX_RATE64]) {
309 		if (priv->shaper != TC_MQPRIO_SHAPER_BW_RATE) {
310 			NL_SET_ERR_MSG_ATTR(extack, tb[TCA_MQPRIO_MAX_RATE64],
311 					    "max_rate accepted only when shaper is in bw_rlimit mode");
312 			return -EINVAL;
313 		}
314 		i = 0;
315 		nla_for_each_nested(attr, tb[TCA_MQPRIO_MAX_RATE64],
316 				    rem) {
317 			if (nla_type(attr) != TCA_MQPRIO_MAX_RATE64) {
318 				NL_SET_ERR_MSG_ATTR(extack, attr,
319 						    "Attribute type expected to be TCA_MQPRIO_MAX_RATE64");
320 				return -EINVAL;
321 			}
322 
323 			if (nla_len(attr) != sizeof(u64)) {
324 				NL_SET_ERR_MSG_ATTR(extack, attr,
325 						    "Attribute TCA_MQPRIO_MAX_RATE64 expected to have 8 bytes length");
326 				return -EINVAL;
327 			}
328 
329 			if (i >= qopt->num_tc)
330 				break;
331 			priv->max_rate[i] = nla_get_u64(attr);
332 			i++;
333 		}
334 		priv->flags |= TC_MQPRIO_F_MAX_RATE;
335 	}
336 
337 	if (tb[TCA_MQPRIO_TC_ENTRY]) {
338 		err = mqprio_parse_tc_entries(sch, nlattr_opt, nlattr_opt_len,
339 					      extack);
340 		if (err)
341 			return err;
342 	}
343 
344 	return 0;
345 }
346 
mqprio_init(struct Qdisc * sch,struct nlattr * opt,struct netlink_ext_ack * extack)347 static int mqprio_init(struct Qdisc *sch, struct nlattr *opt,
348 		       struct netlink_ext_ack *extack)
349 {
350 	struct net_device *dev = qdisc_dev(sch);
351 	struct mqprio_sched *priv = qdisc_priv(sch);
352 	struct netdev_queue *dev_queue;
353 	struct Qdisc *qdisc;
354 	int i, err = -EOPNOTSUPP;
355 	struct tc_mqprio_qopt *qopt = NULL;
356 	struct tc_mqprio_caps caps;
357 	int len, tc;
358 
359 	BUILD_BUG_ON(TC_MAX_QUEUE != TC_QOPT_MAX_QUEUE);
360 	BUILD_BUG_ON(TC_BITMASK != TC_QOPT_BITMASK);
361 
362 	if (sch->parent != TC_H_ROOT)
363 		return -EOPNOTSUPP;
364 
365 	if (!netif_is_multiqueue(dev))
366 		return -EOPNOTSUPP;
367 
368 	/* make certain can allocate enough classids to handle queues */
369 	if (dev->num_tx_queues >= TC_H_MIN_PRIORITY)
370 		return -ENOMEM;
371 
372 	if (!opt || nla_len(opt) < sizeof(*qopt))
373 		return -EINVAL;
374 
375 	for (tc = 0; tc < TC_QOPT_MAX_QUEUE; tc++)
376 		priv->fp[tc] = TC_FP_EXPRESS;
377 
378 	qdisc_offload_query_caps(dev, TC_SETUP_QDISC_MQPRIO,
379 				 &caps, sizeof(caps));
380 
381 	qopt = nla_data(opt);
382 	if (mqprio_parse_opt(dev, qopt, &caps, extack))
383 		return -EINVAL;
384 
385 	len = nla_len(opt) - NLA_ALIGN(sizeof(*qopt));
386 	if (len > 0) {
387 		err = mqprio_parse_nlattr(sch, qopt, opt, extack);
388 		if (err)
389 			return err;
390 	}
391 
392 	/* pre-allocate qdisc, attachment can't fail */
393 	priv->qdiscs = kcalloc(dev->num_tx_queues, sizeof(priv->qdiscs[0]),
394 			       GFP_KERNEL);
395 	if (!priv->qdiscs)
396 		return -ENOMEM;
397 
398 	for (i = 0; i < dev->num_tx_queues; i++) {
399 		dev_queue = netdev_get_tx_queue(dev, i);
400 		qdisc = qdisc_create_dflt(dev_queue,
401 					  get_default_qdisc_ops(dev, i),
402 					  TC_H_MAKE(TC_H_MAJ(sch->handle),
403 						    TC_H_MIN(i + 1)), extack);
404 		if (!qdisc)
405 			return -ENOMEM;
406 
407 		priv->qdiscs[i] = qdisc;
408 		qdisc->flags |= TCQ_F_ONETXQUEUE | TCQ_F_NOPARENT;
409 	}
410 
411 	/* If the mqprio options indicate that hardware should own
412 	 * the queue mapping then run ndo_setup_tc otherwise use the
413 	 * supplied and verified mapping
414 	 */
415 	if (qopt->hw) {
416 		err = mqprio_enable_offload(sch, qopt, extack);
417 		if (err)
418 			return err;
419 	} else {
420 		netdev_set_num_tc(dev, qopt->num_tc);
421 		for (i = 0; i < qopt->num_tc; i++)
422 			netdev_set_tc_queue(dev, i,
423 					    qopt->count[i], qopt->offset[i]);
424 	}
425 
426 	/* Always use supplied priority mappings */
427 	for (i = 0; i < TC_BITMASK + 1; i++)
428 		netdev_set_prio_tc_map(dev, i, qopt->prio_tc_map[i]);
429 
430 	sch->flags |= TCQ_F_MQROOT;
431 	return 0;
432 }
433 
mqprio_attach(struct Qdisc * sch)434 static void mqprio_attach(struct Qdisc *sch)
435 {
436 	struct net_device *dev = qdisc_dev(sch);
437 	struct mqprio_sched *priv = qdisc_priv(sch);
438 	struct Qdisc *qdisc, *old;
439 	unsigned int ntx;
440 
441 	/* Attach underlying qdisc */
442 	for (ntx = 0; ntx < dev->num_tx_queues; ntx++) {
443 		qdisc = priv->qdiscs[ntx];
444 		old = dev_graft_qdisc(qdisc->dev_queue, qdisc);
445 		if (old)
446 			qdisc_put(old);
447 		if (ntx < dev->real_num_tx_queues)
448 			qdisc_hash_add(qdisc, false);
449 	}
450 	kfree(priv->qdiscs);
451 	priv->qdiscs = NULL;
452 }
453 
mqprio_queue_get(struct Qdisc * sch,unsigned long cl)454 static struct netdev_queue *mqprio_queue_get(struct Qdisc *sch,
455 					     unsigned long cl)
456 {
457 	struct net_device *dev = qdisc_dev(sch);
458 	unsigned long ntx = cl - 1;
459 
460 	if (ntx >= dev->num_tx_queues)
461 		return NULL;
462 	return netdev_get_tx_queue(dev, ntx);
463 }
464 
mqprio_graft(struct Qdisc * sch,unsigned long cl,struct Qdisc * new,struct Qdisc ** old,struct netlink_ext_ack * extack)465 static int mqprio_graft(struct Qdisc *sch, unsigned long cl, struct Qdisc *new,
466 			struct Qdisc **old, struct netlink_ext_ack *extack)
467 {
468 	struct net_device *dev = qdisc_dev(sch);
469 	struct netdev_queue *dev_queue = mqprio_queue_get(sch, cl);
470 
471 	if (!dev_queue)
472 		return -EINVAL;
473 
474 	if (dev->flags & IFF_UP)
475 		dev_deactivate(dev);
476 
477 	*old = dev_graft_qdisc(dev_queue, new);
478 
479 	if (new)
480 		new->flags |= TCQ_F_ONETXQUEUE | TCQ_F_NOPARENT;
481 
482 	if (dev->flags & IFF_UP)
483 		dev_activate(dev);
484 
485 	return 0;
486 }
487 
dump_rates(struct mqprio_sched * priv,struct tc_mqprio_qopt * opt,struct sk_buff * skb)488 static int dump_rates(struct mqprio_sched *priv,
489 		      struct tc_mqprio_qopt *opt, struct sk_buff *skb)
490 {
491 	struct nlattr *nest;
492 	int i;
493 
494 	if (priv->flags & TC_MQPRIO_F_MIN_RATE) {
495 		nest = nla_nest_start_noflag(skb, TCA_MQPRIO_MIN_RATE64);
496 		if (!nest)
497 			goto nla_put_failure;
498 
499 		for (i = 0; i < opt->num_tc; i++) {
500 			if (nla_put(skb, TCA_MQPRIO_MIN_RATE64,
501 				    sizeof(priv->min_rate[i]),
502 				    &priv->min_rate[i]))
503 				goto nla_put_failure;
504 		}
505 		nla_nest_end(skb, nest);
506 	}
507 
508 	if (priv->flags & TC_MQPRIO_F_MAX_RATE) {
509 		nest = nla_nest_start_noflag(skb, TCA_MQPRIO_MAX_RATE64);
510 		if (!nest)
511 			goto nla_put_failure;
512 
513 		for (i = 0; i < opt->num_tc; i++) {
514 			if (nla_put(skb, TCA_MQPRIO_MAX_RATE64,
515 				    sizeof(priv->max_rate[i]),
516 				    &priv->max_rate[i]))
517 				goto nla_put_failure;
518 		}
519 		nla_nest_end(skb, nest);
520 	}
521 	return 0;
522 
523 nla_put_failure:
524 	nla_nest_cancel(skb, nest);
525 	return -1;
526 }
527 
mqprio_dump_tc_entries(struct mqprio_sched * priv,struct sk_buff * skb)528 static int mqprio_dump_tc_entries(struct mqprio_sched *priv,
529 				  struct sk_buff *skb)
530 {
531 	struct nlattr *n;
532 	int tc;
533 
534 	for (tc = 0; tc < TC_QOPT_MAX_QUEUE; tc++) {
535 		n = nla_nest_start(skb, TCA_MQPRIO_TC_ENTRY);
536 		if (!n)
537 			return -EMSGSIZE;
538 
539 		if (nla_put_u32(skb, TCA_MQPRIO_TC_ENTRY_INDEX, tc))
540 			goto nla_put_failure;
541 
542 		if (nla_put_u32(skb, TCA_MQPRIO_TC_ENTRY_FP, priv->fp[tc]))
543 			goto nla_put_failure;
544 
545 		nla_nest_end(skb, n);
546 	}
547 
548 	return 0;
549 
550 nla_put_failure:
551 	nla_nest_cancel(skb, n);
552 	return -EMSGSIZE;
553 }
554 
mqprio_dump(struct Qdisc * sch,struct sk_buff * skb)555 static int mqprio_dump(struct Qdisc *sch, struct sk_buff *skb)
556 {
557 	struct net_device *dev = qdisc_dev(sch);
558 	struct mqprio_sched *priv = qdisc_priv(sch);
559 	struct nlattr *nla = (struct nlattr *)skb_tail_pointer(skb);
560 	struct tc_mqprio_qopt opt = { 0 };
561 	struct Qdisc *qdisc;
562 	unsigned int ntx;
563 
564 	sch->q.qlen = 0;
565 	gnet_stats_basic_sync_init(&sch->bstats);
566 	memset(&sch->qstats, 0, sizeof(sch->qstats));
567 
568 	/* MQ supports lockless qdiscs. However, statistics accounting needs
569 	 * to account for all, none, or a mix of locked and unlocked child
570 	 * qdiscs. Percpu stats are added to counters in-band and locking
571 	 * qdisc totals are added at end.
572 	 */
573 	for (ntx = 0; ntx < dev->num_tx_queues; ntx++) {
574 		qdisc = rtnl_dereference(netdev_get_tx_queue(dev, ntx)->qdisc_sleeping);
575 		spin_lock_bh(qdisc_lock(qdisc));
576 
577 		gnet_stats_add_basic(&sch->bstats, qdisc->cpu_bstats,
578 				     &qdisc->bstats, false);
579 		gnet_stats_add_queue(&sch->qstats, qdisc->cpu_qstats,
580 				     &qdisc->qstats);
581 		sch->q.qlen += qdisc_qlen(qdisc);
582 
583 		spin_unlock_bh(qdisc_lock(qdisc));
584 	}
585 
586 	mqprio_qopt_reconstruct(dev, &opt);
587 	opt.hw = priv->hw_offload;
588 
589 	if (nla_put(skb, TCA_OPTIONS, sizeof(opt), &opt))
590 		goto nla_put_failure;
591 
592 	if ((priv->flags & TC_MQPRIO_F_MODE) &&
593 	    nla_put_u16(skb, TCA_MQPRIO_MODE, priv->mode))
594 		goto nla_put_failure;
595 
596 	if ((priv->flags & TC_MQPRIO_F_SHAPER) &&
597 	    nla_put_u16(skb, TCA_MQPRIO_SHAPER, priv->shaper))
598 		goto nla_put_failure;
599 
600 	if ((priv->flags & TC_MQPRIO_F_MIN_RATE ||
601 	     priv->flags & TC_MQPRIO_F_MAX_RATE) &&
602 	    (dump_rates(priv, &opt, skb) != 0))
603 		goto nla_put_failure;
604 
605 	if (mqprio_dump_tc_entries(priv, skb))
606 		goto nla_put_failure;
607 
608 	return nla_nest_end(skb, nla);
609 nla_put_failure:
610 	nlmsg_trim(skb, nla);
611 	return -1;
612 }
613 
mqprio_leaf(struct Qdisc * sch,unsigned long cl)614 static struct Qdisc *mqprio_leaf(struct Qdisc *sch, unsigned long cl)
615 {
616 	struct netdev_queue *dev_queue = mqprio_queue_get(sch, cl);
617 
618 	if (!dev_queue)
619 		return NULL;
620 
621 	return rtnl_dereference(dev_queue->qdisc_sleeping);
622 }
623 
mqprio_find(struct Qdisc * sch,u32 classid)624 static unsigned long mqprio_find(struct Qdisc *sch, u32 classid)
625 {
626 	struct net_device *dev = qdisc_dev(sch);
627 	unsigned int ntx = TC_H_MIN(classid);
628 
629 	/* There are essentially two regions here that have valid classid
630 	 * values. The first region will have a classid value of 1 through
631 	 * num_tx_queues. All of these are backed by actual Qdiscs.
632 	 */
633 	if (ntx < TC_H_MIN_PRIORITY)
634 		return (ntx <= dev->num_tx_queues) ? ntx : 0;
635 
636 	/* The second region represents the hardware traffic classes. These
637 	 * are represented by classid values of TC_H_MIN_PRIORITY through
638 	 * TC_H_MIN_PRIORITY + netdev_get_num_tc - 1
639 	 */
640 	return ((ntx - TC_H_MIN_PRIORITY) < netdev_get_num_tc(dev)) ? ntx : 0;
641 }
642 
mqprio_dump_class(struct Qdisc * sch,unsigned long cl,struct sk_buff * skb,struct tcmsg * tcm)643 static int mqprio_dump_class(struct Qdisc *sch, unsigned long cl,
644 			 struct sk_buff *skb, struct tcmsg *tcm)
645 {
646 	if (cl < TC_H_MIN_PRIORITY) {
647 		struct netdev_queue *dev_queue = mqprio_queue_get(sch, cl);
648 		struct net_device *dev = qdisc_dev(sch);
649 		int tc = netdev_txq_to_tc(dev, cl - 1);
650 
651 		tcm->tcm_parent = (tc < 0) ? 0 :
652 			TC_H_MAKE(TC_H_MAJ(sch->handle),
653 				  TC_H_MIN(tc + TC_H_MIN_PRIORITY));
654 		tcm->tcm_info = rtnl_dereference(dev_queue->qdisc_sleeping)->handle;
655 	} else {
656 		tcm->tcm_parent = TC_H_ROOT;
657 		tcm->tcm_info = 0;
658 	}
659 	tcm->tcm_handle |= TC_H_MIN(cl);
660 	return 0;
661 }
662 
mqprio_dump_class_stats(struct Qdisc * sch,unsigned long cl,struct gnet_dump * d)663 static int mqprio_dump_class_stats(struct Qdisc *sch, unsigned long cl,
664 				   struct gnet_dump *d)
665 	__releases(d->lock)
666 	__acquires(d->lock)
667 {
668 	if (cl >= TC_H_MIN_PRIORITY) {
669 		int i;
670 		__u32 qlen;
671 		struct gnet_stats_queue qstats = {0};
672 		struct gnet_stats_basic_sync bstats;
673 		struct net_device *dev = qdisc_dev(sch);
674 		struct netdev_tc_txq tc = dev->tc_to_txq[cl & TC_BITMASK];
675 
676 		gnet_stats_basic_sync_init(&bstats);
677 		/* Drop lock here it will be reclaimed before touching
678 		 * statistics this is required because the d->lock we
679 		 * hold here is the look on dev_queue->qdisc_sleeping
680 		 * also acquired below.
681 		 */
682 		if (d->lock)
683 			spin_unlock_bh(d->lock);
684 
685 		for (i = tc.offset; i < tc.offset + tc.count; i++) {
686 			struct netdev_queue *q = netdev_get_tx_queue(dev, i);
687 			struct Qdisc *qdisc = rtnl_dereference(q->qdisc);
688 
689 			spin_lock_bh(qdisc_lock(qdisc));
690 
691 			gnet_stats_add_basic(&bstats, qdisc->cpu_bstats,
692 					     &qdisc->bstats, false);
693 			gnet_stats_add_queue(&qstats, qdisc->cpu_qstats,
694 					     &qdisc->qstats);
695 			sch->q.qlen += qdisc_qlen(qdisc);
696 
697 			spin_unlock_bh(qdisc_lock(qdisc));
698 		}
699 		qlen = qdisc_qlen(sch) + qstats.qlen;
700 
701 		/* Reclaim root sleeping lock before completing stats */
702 		if (d->lock)
703 			spin_lock_bh(d->lock);
704 		if (gnet_stats_copy_basic(d, NULL, &bstats, false) < 0 ||
705 		    gnet_stats_copy_queue(d, NULL, &qstats, qlen) < 0)
706 			return -1;
707 	} else {
708 		struct netdev_queue *dev_queue = mqprio_queue_get(sch, cl);
709 
710 		sch = rtnl_dereference(dev_queue->qdisc_sleeping);
711 		if (gnet_stats_copy_basic(d, sch->cpu_bstats,
712 					  &sch->bstats, true) < 0 ||
713 		    qdisc_qstats_copy(d, sch) < 0)
714 			return -1;
715 	}
716 	return 0;
717 }
718 
mqprio_walk(struct Qdisc * sch,struct qdisc_walker * arg)719 static void mqprio_walk(struct Qdisc *sch, struct qdisc_walker *arg)
720 {
721 	struct net_device *dev = qdisc_dev(sch);
722 	unsigned long ntx;
723 
724 	if (arg->stop)
725 		return;
726 
727 	/* Walk hierarchy with a virtual class per tc */
728 	arg->count = arg->skip;
729 	for (ntx = arg->skip; ntx < netdev_get_num_tc(dev); ntx++) {
730 		if (!tc_qdisc_stats_dump(sch, ntx + TC_H_MIN_PRIORITY, arg))
731 			return;
732 	}
733 
734 	/* Pad the values and skip over unused traffic classes */
735 	if (ntx < TC_MAX_QUEUE) {
736 		arg->count = TC_MAX_QUEUE;
737 		ntx = TC_MAX_QUEUE;
738 	}
739 
740 	/* Reset offset, sort out remaining per-queue qdiscs */
741 	for (ntx -= TC_MAX_QUEUE; ntx < dev->num_tx_queues; ntx++) {
742 		if (arg->fn(sch, ntx + 1, arg) < 0) {
743 			arg->stop = 1;
744 			return;
745 		}
746 		arg->count++;
747 	}
748 }
749 
mqprio_select_queue(struct Qdisc * sch,struct tcmsg * tcm)750 static struct netdev_queue *mqprio_select_queue(struct Qdisc *sch,
751 						struct tcmsg *tcm)
752 {
753 	return mqprio_queue_get(sch, TC_H_MIN(tcm->tcm_parent));
754 }
755 
756 static const struct Qdisc_class_ops mqprio_class_ops = {
757 	.graft		= mqprio_graft,
758 	.leaf		= mqprio_leaf,
759 	.find		= mqprio_find,
760 	.walk		= mqprio_walk,
761 	.dump		= mqprio_dump_class,
762 	.dump_stats	= mqprio_dump_class_stats,
763 	.select_queue	= mqprio_select_queue,
764 };
765 
766 static struct Qdisc_ops mqprio_qdisc_ops __read_mostly = {
767 	.cl_ops		= &mqprio_class_ops,
768 	.id		= "mqprio",
769 	.priv_size	= sizeof(struct mqprio_sched),
770 	.init		= mqprio_init,
771 	.destroy	= mqprio_destroy,
772 	.attach		= mqprio_attach,
773 	.change_real_num_tx = mq_change_real_num_tx,
774 	.dump		= mqprio_dump,
775 	.owner		= THIS_MODULE,
776 };
777 
mqprio_module_init(void)778 static int __init mqprio_module_init(void)
779 {
780 	return register_qdisc(&mqprio_qdisc_ops);
781 }
782 
mqprio_module_exit(void)783 static void __exit mqprio_module_exit(void)
784 {
785 	unregister_qdisc(&mqprio_qdisc_ops);
786 }
787 
788 module_init(mqprio_module_init);
789 module_exit(mqprio_module_exit);
790 
791 MODULE_LICENSE("GPL");
792