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 | |
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(qdisc: 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(fp: priv->fp, mqprio: &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 | |
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(qdisc: 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 | |
95 | static void mqprio_destroy(struct Qdisc *sch) |
96 | { |
97 | struct net_device *dev = qdisc_dev(qdisc: 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(qdisc: priv->qdiscs[ntx]); |
106 | kfree(objp: 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, num_tc: 0); |
113 | } |
114 | |
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 | validate_queue_counts: !qopt->hw || caps->validate_queue_counts, |
136 | allow_overlapping_txqs: 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 | |
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, maxtype: TCA_MQPRIO_TC_ENTRY_MAX, nla: opt, |
178 | policy: 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(nla: 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(nla: tb[TCA_MQPRIO_TC_ENTRY_FP]); |
198 | |
199 | return 0; |
200 | } |
201 | |
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(qdisc: 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(nla: n) != TCA_MQPRIO_TC_ENTRY) |
220 | continue; |
221 | |
222 | err = mqprio_parse_tc_entry(fp, opt: n, seen_tcs: &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 | */ |
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(nla: opt) + NLA_ALIGN(sizeof(*qopt)); |
249 | int nlattr_opt_len = nla_len(nla: 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(payload: 0)) { |
256 | err = nla_parse_deprecated(tb, TCA_MQPRIO_MAX, head: nlattr_opt, |
257 | len: nlattr_opt_len, policy: 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(nla: 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(nla: 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(nla: 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(nla: 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(nla: 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(nla: 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(nla: 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(nla: 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 | |
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(qdisc: 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(nla: 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, type: TC_SETUP_QDISC_MQPRIO, |
379 | caps: &caps, caps_len: sizeof(caps)); |
380 | |
381 | qopt = nla_data(nla: opt); |
382 | if (mqprio_parse_opt(dev, qopt, caps: &caps, extack)) |
383 | return -EINVAL; |
384 | |
385 | len = nla_len(nla: 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(n: dev->num_tx_queues, size: 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, index: i); |
400 | qdisc = qdisc_create_dflt(dev_queue, |
401 | ops: get_default_qdisc_ops(dev, ntx: 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, num_tc: qopt->num_tc); |
421 | for (i = 0; i < qopt->num_tc; i++) |
422 | netdev_set_tc_queue(dev, tc: i, |
423 | count: qopt->count[i], offset: 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, prio: i, tc: qopt->prio_tc_map[i]); |
429 | |
430 | sch->flags |= TCQ_F_MQROOT; |
431 | return 0; |
432 | } |
433 | |
434 | static void mqprio_attach(struct Qdisc *sch) |
435 | { |
436 | struct net_device *dev = qdisc_dev(qdisc: 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(dev_queue: qdisc->dev_queue, qdisc); |
445 | if (old) |
446 | qdisc_put(qdisc: old); |
447 | if (ntx < dev->real_num_tx_queues) |
448 | qdisc_hash_add(q: qdisc, invisible: false); |
449 | } |
450 | kfree(objp: priv->qdiscs); |
451 | priv->qdiscs = NULL; |
452 | } |
453 | |
454 | static struct netdev_queue *mqprio_queue_get(struct Qdisc *sch, |
455 | unsigned long cl) |
456 | { |
457 | struct net_device *dev = qdisc_dev(qdisc: 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, index: ntx); |
463 | } |
464 | |
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(qdisc: 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, qdisc: 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 | |
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, attrtype: 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, attrtype: TCA_MQPRIO_MIN_RATE64, |
501 | attrlen: sizeof(priv->min_rate[i]), |
502 | data: &priv->min_rate[i])) |
503 | goto nla_put_failure; |
504 | } |
505 | nla_nest_end(skb, start: nest); |
506 | } |
507 | |
508 | if (priv->flags & TC_MQPRIO_F_MAX_RATE) { |
509 | nest = nla_nest_start_noflag(skb, attrtype: 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, attrtype: TCA_MQPRIO_MAX_RATE64, |
515 | attrlen: sizeof(priv->max_rate[i]), |
516 | data: &priv->max_rate[i])) |
517 | goto nla_put_failure; |
518 | } |
519 | nla_nest_end(skb, start: nest); |
520 | } |
521 | return 0; |
522 | |
523 | nla_put_failure: |
524 | nla_nest_cancel(skb, start: nest); |
525 | return -1; |
526 | } |
527 | |
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, attrtype: TCA_MQPRIO_TC_ENTRY); |
536 | if (!n) |
537 | return -EMSGSIZE; |
538 | |
539 | if (nla_put_u32(skb, attrtype: TCA_MQPRIO_TC_ENTRY_INDEX, value: tc)) |
540 | goto nla_put_failure; |
541 | |
542 | if (nla_put_u32(skb, attrtype: TCA_MQPRIO_TC_ENTRY_FP, value: priv->fp[tc])) |
543 | goto nla_put_failure; |
544 | |
545 | nla_nest_end(skb, start: n); |
546 | } |
547 | |
548 | return 0; |
549 | |
550 | nla_put_failure: |
551 | nla_nest_cancel(skb, start: n); |
552 | return -EMSGSIZE; |
553 | } |
554 | |
555 | static int mqprio_dump(struct Qdisc *sch, struct sk_buff *skb) |
556 | { |
557 | struct net_device *dev = qdisc_dev(qdisc: 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(b: &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(lock: qdisc_lock(qdisc)); |
576 | |
577 | gnet_stats_add_basic(bstats: &sch->bstats, cpu: qdisc->cpu_bstats, |
578 | b: &qdisc->bstats, running: false); |
579 | gnet_stats_add_queue(qstats: &sch->qstats, cpu_q: qdisc->cpu_qstats, |
580 | q: &qdisc->qstats); |
581 | sch->q.qlen += qdisc_qlen(q: qdisc); |
582 | |
583 | spin_unlock_bh(lock: qdisc_lock(qdisc)); |
584 | } |
585 | |
586 | mqprio_qopt_reconstruct(dev, qopt: &opt); |
587 | opt.hw = priv->hw_offload; |
588 | |
589 | if (nla_put(skb, attrtype: TCA_OPTIONS, attrlen: sizeof(opt), data: &opt)) |
590 | goto nla_put_failure; |
591 | |
592 | if ((priv->flags & TC_MQPRIO_F_MODE) && |
593 | nla_put_u16(skb, attrtype: TCA_MQPRIO_MODE, value: priv->mode)) |
594 | goto nla_put_failure; |
595 | |
596 | if ((priv->flags & TC_MQPRIO_F_SHAPER) && |
597 | nla_put_u16(skb, attrtype: TCA_MQPRIO_SHAPER, value: 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: &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, start: nla); |
609 | nla_put_failure: |
610 | nlmsg_trim(skb, mark: nla); |
611 | return -1; |
612 | } |
613 | |
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 | |
624 | static unsigned long mqprio_find(struct Qdisc *sch, u32 classid) |
625 | { |
626 | struct net_device *dev = qdisc_dev(qdisc: 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 | |
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(qdisc: sch); |
649 | int tc = netdev_txq_to_tc(dev, txq: 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 | |
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(qdisc: sch); |
674 | struct netdev_tc_txq tc = dev->tc_to_txq[cl & TC_BITMASK]; |
675 | |
676 | gnet_stats_basic_sync_init(b: &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(lock: d->lock); |
684 | |
685 | for (i = tc.offset; i < tc.offset + tc.count; i++) { |
686 | struct netdev_queue *q = netdev_get_tx_queue(dev, index: i); |
687 | struct Qdisc *qdisc = rtnl_dereference(q->qdisc); |
688 | |
689 | spin_lock_bh(lock: qdisc_lock(qdisc)); |
690 | |
691 | gnet_stats_add_basic(bstats: &bstats, cpu: qdisc->cpu_bstats, |
692 | b: &qdisc->bstats, running: false); |
693 | gnet_stats_add_queue(qstats: &qstats, cpu_q: qdisc->cpu_qstats, |
694 | q: &qdisc->qstats); |
695 | sch->q.qlen += qdisc_qlen(q: qdisc); |
696 | |
697 | spin_unlock_bh(lock: qdisc_lock(qdisc)); |
698 | } |
699 | qlen = qdisc_qlen(q: sch) + qstats.qlen; |
700 | |
701 | /* Reclaim root sleeping lock before completing stats */ |
702 | if (d->lock) |
703 | spin_lock_bh(lock: d->lock); |
704 | if (gnet_stats_copy_basic(d, NULL, b: &bstats, running: false) < 0 || |
705 | gnet_stats_copy_queue(d, NULL, q: &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, cpu: sch->cpu_bstats, |
712 | b: &sch->bstats, running: true) < 0 || |
713 | qdisc_qstats_copy(d, sch) < 0) |
714 | return -1; |
715 | } |
716 | return 0; |
717 | } |
718 | |
719 | static void mqprio_walk(struct Qdisc *sch, struct qdisc_walker *arg) |
720 | { |
721 | struct net_device *dev = qdisc_dev(qdisc: 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, cl: 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 | |
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 | |
778 | static int __init mqprio_module_init(void) |
779 | { |
780 | return register_qdisc(qops: &mqprio_qdisc_ops); |
781 | } |
782 | |
783 | static void __exit mqprio_module_exit(void) |
784 | { |
785 | unregister_qdisc(qops: &mqprio_qdisc_ops); |
786 | } |
787 | |
788 | module_init(mqprio_module_init); |
789 | module_exit(mqprio_module_exit); |
790 | |
791 | MODULE_LICENSE("GPL" ); |
792 | |