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
 23struct 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
 34static 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
 80static 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
 95static 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
115static 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
152static const struct
153nla_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
161static 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
169static 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
202static 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	}
237out:
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 */
244static 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
347static 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
434static 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
454static 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
465static 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
488static 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
523nla_put_failure:
524	nla_nest_cancel(skb, nest);
525	return -1;
526}
527
528static 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
550nla_put_failure:
551	nla_nest_cancel(skb, n);
552	return -EMSGSIZE;
553}
554
555static 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);
609nla_put_failure:
610	nlmsg_trim(skb, nla);
611	return -1;
612}
613
614static 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
624static 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
643static 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
663static 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
719static 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
750static 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
756static 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
766static 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
778static int __init mqprio_module_init(void)
779{
780	return register_qdisc(&mqprio_qdisc_ops);
781}
782
783static void __exit mqprio_module_exit(void)
784{
785	unregister_qdisc(&mqprio_qdisc_ops);
786}
787
788module_init(mqprio_module_init);
789module_exit(mqprio_module_exit);
790
791MODULE_LICENSE("GPL");
792MODULE_DESCRIPTION("Classful multiqueue prio qdisc");