Blame view

net/sched/sch_mqprio.c 16.6 KB
d2912cb15   Thomas Gleixner   treewide: Replace...
1
  // SPDX-License-Identifier: GPL-2.0-only
b8970f0bf   John Fastabend   net_sched: implem...
2
3
4
5
  /*
   * net/sched/sch_mqprio.c
   *
   * Copyright (c) 2010 John Fastabend <john.r.fastabend@intel.com>
b8970f0bf   John Fastabend   net_sched: implem...
6
7
8
9
10
11
12
13
   */
  
  #include <linux/types.h>
  #include <linux/slab.h>
  #include <linux/kernel.h>
  #include <linux/string.h>
  #include <linux/errno.h>
  #include <linux/skbuff.h>
3a9a231d9   Paul Gortmaker   net: Fix files ex...
14
  #include <linux/module.h>
b8970f0bf   John Fastabend   net_sched: implem...
15
16
17
  #include <net/netlink.h>
  #include <net/pkt_sched.h>
  #include <net/sch_generic.h>
4e8b86c06   Amritha Nambiar   mqprio: Introduce...
18
  #include <net/pkt_cls.h>
b8970f0bf   John Fastabend   net_sched: implem...
19
20
21
  
  struct mqprio_sched {
  	struct Qdisc		**qdiscs;
4e8b86c06   Amritha Nambiar   mqprio: Introduce...
22
23
  	u16 mode;
  	u16 shaper;
2026fecf5   Alexander Duyck   mqprio: Change ha...
24
  	int hw_offload;
4e8b86c06   Amritha Nambiar   mqprio: Introduce...
25
26
27
  	u32 flags;
  	u64 min_rate[TC_QOPT_MAX_QUEUE];
  	u64 max_rate[TC_QOPT_MAX_QUEUE];
b8970f0bf   John Fastabend   net_sched: implem...
28
29
30
31
32
33
34
  };
  
  static void mqprio_destroy(struct Qdisc *sch)
  {
  	struct net_device *dev = qdisc_dev(sch);
  	struct mqprio_sched *priv = qdisc_priv(sch);
  	unsigned int ntx;
ac7100ba9   Ben Hutchings   sch_mqprio: Alway...
35
36
37
38
  	if (priv->qdiscs) {
  		for (ntx = 0;
  		     ntx < dev->num_tx_queues && priv->qdiscs[ntx];
  		     ntx++)
86bd446b5   Vlad Buslov   net: sched: renam...
39
  			qdisc_put(priv->qdiscs[ntx]);
ac7100ba9   Ben Hutchings   sch_mqprio: Alway...
40
41
  		kfree(priv->qdiscs);
  	}
b8970f0bf   John Fastabend   net_sched: implem...
42

56f36acd2   Amritha Nambiar   mqprio: Modify mq...
43
  	if (priv->hw_offload && dev->netdev_ops->ndo_setup_tc) {
4e8b86c06   Amritha Nambiar   mqprio: Introduce...
44
45
46
47
48
  		struct tc_mqprio_qopt_offload mqprio = { { 0 } };
  
  		switch (priv->mode) {
  		case TC_MQPRIO_MODE_DCB:
  		case TC_MQPRIO_MODE_CHANNEL:
575ed7d39   Nogah Frankel   net_sch: mqprio: ...
49
50
  			dev->netdev_ops->ndo_setup_tc(dev,
  						      TC_SETUP_QDISC_MQPRIO,
4e8b86c06   Amritha Nambiar   mqprio: Introduce...
51
52
53
54
55
  						      &mqprio);
  			break;
  		default:
  			return;
  		}
56f36acd2   Amritha Nambiar   mqprio: Modify mq...
56
  	} else {
b8970f0bf   John Fastabend   net_sched: implem...
57
  		netdev_set_num_tc(dev, 0);
56f36acd2   Amritha Nambiar   mqprio: Modify mq...
58
  	}
b8970f0bf   John Fastabend   net_sched: implem...
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
  }
  
  static int mqprio_parse_opt(struct net_device *dev, struct tc_mqprio_qopt *qopt)
  {
  	int i, j;
  
  	/* Verify num_tc is not out of max range */
  	if (qopt->num_tc > TC_MAX_QUEUE)
  		return -EINVAL;
  
  	/* Verify priority mapping uses valid tcs */
  	for (i = 0; i < TC_BITMASK + 1; i++) {
  		if (qopt->prio_tc_map[i] >= qopt->num_tc)
  			return -EINVAL;
  	}
2026fecf5   Alexander Duyck   mqprio: Change ha...
74
75
76
77
78
79
  	/* Limit qopt->hw to maximum supported offload value.  Drivers have
  	 * the option of overriding this later if they don't support the a
  	 * given offload type.
  	 */
  	if (qopt->hw > TC_MQPRIO_HW_OFFLOAD_MAX)
  		qopt->hw = TC_MQPRIO_HW_OFFLOAD_MAX;
b8970f0bf   John Fastabend   net_sched: implem...
80

2026fecf5   Alexander Duyck   mqprio: Change ha...
81
82
83
84
  	/* If hardware offload is requested we will leave it to the device
  	 * to either populate the queue counts itself or to validate the
  	 * provided queue counts.  If ndo_setup_tc is not present then
  	 * hardware doesn't support offload and we should return an error.
b8970f0bf   John Fastabend   net_sched: implem...
85
86
  	 */
  	if (qopt->hw)
2026fecf5   Alexander Duyck   mqprio: Change ha...
87
  		return dev->netdev_ops->ndo_setup_tc ? 0 : -EINVAL;
b8970f0bf   John Fastabend   net_sched: implem...
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
  
  	for (i = 0; i < qopt->num_tc; i++) {
  		unsigned int last = qopt->offset[i] + qopt->count[i];
  
  		/* Verify the queue count is in tx range being equal to the
  		 * real_num_tx_queues indicates the last queue is in use.
  		 */
  		if (qopt->offset[i] >= dev->real_num_tx_queues ||
  		    !qopt->count[i] ||
  		    last > dev->real_num_tx_queues)
  			return -EINVAL;
  
  		/* Verify that the offset and counts do not overlap */
  		for (j = i + 1; j < qopt->num_tc; j++) {
  			if (last > qopt->offset[j])
  				return -EINVAL;
  		}
  	}
  
  	return 0;
  }
4e8b86c06   Amritha Nambiar   mqprio: Introduce...
109
110
111
112
113
114
115
116
117
118
119
120
121
  static const struct nla_policy mqprio_policy[TCA_MQPRIO_MAX + 1] = {
  	[TCA_MQPRIO_MODE]	= { .len = sizeof(u16) },
  	[TCA_MQPRIO_SHAPER]	= { .len = sizeof(u16) },
  	[TCA_MQPRIO_MIN_RATE64]	= { .type = NLA_NESTED },
  	[TCA_MQPRIO_MAX_RATE64]	= { .type = NLA_NESTED },
  };
  
  static int parse_attr(struct nlattr *tb[], int maxtype, struct nlattr *nla,
  		      const struct nla_policy *policy, int len)
  {
  	int nested_len = nla_len(nla) - NLA_ALIGN(len);
  
  	if (nested_len >= nla_attr_size(0))
8cb081746   Johannes Berg   netlink: make val...
122
123
124
  		return nla_parse_deprecated(tb, maxtype,
  					    nla_data(nla) + NLA_ALIGN(len),
  					    nested_len, policy, NULL);
4e8b86c06   Amritha Nambiar   mqprio: Introduce...
125
126
127
128
  
  	memset(tb, 0, sizeof(struct nlattr *) * (maxtype + 1));
  	return 0;
  }
e63d7dfd2   Alexander Aring   net: sched: sch: ...
129
130
  static int mqprio_init(struct Qdisc *sch, struct nlattr *opt,
  		       struct netlink_ext_ack *extack)
b8970f0bf   John Fastabend   net_sched: implem...
131
132
133
134
135
136
137
  {
  	struct net_device *dev = qdisc_dev(sch);
  	struct mqprio_sched *priv = qdisc_priv(sch);
  	struct netdev_queue *dev_queue;
  	struct Qdisc *qdisc;
  	int i, err = -EOPNOTSUPP;
  	struct tc_mqprio_qopt *qopt = NULL;
4e8b86c06   Amritha Nambiar   mqprio: Introduce...
138
139
140
  	struct nlattr *tb[TCA_MQPRIO_MAX + 1];
  	struct nlattr *attr;
  	int rem;
22ce97fe4   Colin Ian King   mqprio: fix poten...
141
  	int len;
b8970f0bf   John Fastabend   net_sched: implem...
142
143
144
145
146
147
148
149
150
  
  	BUILD_BUG_ON(TC_MAX_QUEUE != TC_QOPT_MAX_QUEUE);
  	BUILD_BUG_ON(TC_BITMASK != TC_QOPT_BITMASK);
  
  	if (sch->parent != TC_H_ROOT)
  		return -EOPNOTSUPP;
  
  	if (!netif_is_multiqueue(dev))
  		return -EOPNOTSUPP;
32302902f   Alexander Duyck   mqprio: Reserve l...
151
152
153
  	/* make certain can allocate enough classids to handle queues */
  	if (dev->num_tx_queues >= TC_H_MIN_PRIORITY)
  		return -ENOMEM;
7838f2ce3   Thomas Graf   mqprio: Avoid pan...
154
  	if (!opt || nla_len(opt) < sizeof(*qopt))
b8970f0bf   John Fastabend   net_sched: implem...
155
156
157
158
159
  		return -EINVAL;
  
  	qopt = nla_data(opt);
  	if (mqprio_parse_opt(dev, qopt))
  		return -EINVAL;
22ce97fe4   Colin Ian King   mqprio: fix poten...
160
  	len = nla_len(opt) - NLA_ALIGN(sizeof(*qopt));
4e8b86c06   Amritha Nambiar   mqprio: Introduce...
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
  	if (len > 0) {
  		err = parse_attr(tb, TCA_MQPRIO_MAX, opt, mqprio_policy,
  				 sizeof(*qopt));
  		if (err < 0)
  			return err;
  
  		if (!qopt->hw)
  			return -EINVAL;
  
  		if (tb[TCA_MQPRIO_MODE]) {
  			priv->flags |= TC_MQPRIO_F_MODE;
  			priv->mode = *(u16 *)nla_data(tb[TCA_MQPRIO_MODE]);
  		}
  
  		if (tb[TCA_MQPRIO_SHAPER]) {
  			priv->flags |= TC_MQPRIO_F_SHAPER;
  			priv->shaper = *(u16 *)nla_data(tb[TCA_MQPRIO_SHAPER]);
  		}
  
  		if (tb[TCA_MQPRIO_MIN_RATE64]) {
  			if (priv->shaper != TC_MQPRIO_SHAPER_BW_RATE)
  				return -EINVAL;
  			i = 0;
  			nla_for_each_nested(attr, tb[TCA_MQPRIO_MIN_RATE64],
  					    rem) {
  				if (nla_type(attr) != TCA_MQPRIO_MIN_RATE64)
  					return -EINVAL;
  				if (i >= qopt->num_tc)
  					break;
  				priv->min_rate[i] = *(u64 *)nla_data(attr);
  				i++;
  			}
  			priv->flags |= TC_MQPRIO_F_MIN_RATE;
  		}
  
  		if (tb[TCA_MQPRIO_MAX_RATE64]) {
  			if (priv->shaper != TC_MQPRIO_SHAPER_BW_RATE)
  				return -EINVAL;
  			i = 0;
  			nla_for_each_nested(attr, tb[TCA_MQPRIO_MAX_RATE64],
  					    rem) {
  				if (nla_type(attr) != TCA_MQPRIO_MAX_RATE64)
  					return -EINVAL;
  				if (i >= qopt->num_tc)
  					break;
  				priv->max_rate[i] = *(u64 *)nla_data(attr);
  				i++;
  			}
  			priv->flags |= TC_MQPRIO_F_MAX_RATE;
  		}
  	}
b8970f0bf   John Fastabend   net_sched: implem...
212
213
214
  	/* pre-allocate qdisc, attachment can't fail */
  	priv->qdiscs = kcalloc(dev->num_tx_queues, sizeof(priv->qdiscs[0]),
  			       GFP_KERNEL);
87b60cfac   Eric Dumazet   net_sched: fix er...
215
216
  	if (!priv->qdiscs)
  		return -ENOMEM;
b8970f0bf   John Fastabend   net_sched: implem...
217
218
219
  
  	for (i = 0; i < dev->num_tx_queues; i++) {
  		dev_queue = netdev_get_tx_queue(dev, i);
1f27cde31   Eric Dumazet   net: sched: use p...
220
221
  		qdisc = qdisc_create_dflt(dev_queue,
  					  get_default_qdisc_ops(dev, i),
b8970f0bf   John Fastabend   net_sched: implem...
222
  					  TC_H_MAKE(TC_H_MAJ(sch->handle),
a38a98821   Alexander Aring   net: sch: api: ad...
223
  						    TC_H_MIN(i + 1)), extack);
87b60cfac   Eric Dumazet   net_sched: fix er...
224
225
  		if (!qdisc)
  			return -ENOMEM;
b8970f0bf   John Fastabend   net_sched: implem...
226
  		priv->qdiscs[i] = qdisc;
4eaf3b84f   Eric Dumazet   net_sched: fix qd...
227
  		qdisc->flags |= TCQ_F_ONETXQUEUE | TCQ_F_NOPARENT;
b8970f0bf   John Fastabend   net_sched: implem...
228
229
230
231
232
233
234
  	}
  
  	/* If the mqprio options indicate that hardware should own
  	 * the queue mapping then run ndo_setup_tc otherwise use the
  	 * supplied and verified mapping
  	 */
  	if (qopt->hw) {
4e8b86c06   Amritha Nambiar   mqprio: Introduce...
235
  		struct tc_mqprio_qopt_offload mqprio = {.qopt = *qopt};
16e5cc647   John Fastabend   net: rework setup...
236

4e8b86c06   Amritha Nambiar   mqprio: Introduce...
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
  		switch (priv->mode) {
  		case TC_MQPRIO_MODE_DCB:
  			if (priv->shaper != TC_MQPRIO_SHAPER_DCB)
  				return -EINVAL;
  			break;
  		case TC_MQPRIO_MODE_CHANNEL:
  			mqprio.flags = priv->flags;
  			if (priv->flags & TC_MQPRIO_F_MODE)
  				mqprio.mode = priv->mode;
  			if (priv->flags & TC_MQPRIO_F_SHAPER)
  				mqprio.shaper = priv->shaper;
  			if (priv->flags & TC_MQPRIO_F_MIN_RATE)
  				for (i = 0; i < mqprio.qopt.num_tc; i++)
  					mqprio.min_rate[i] = priv->min_rate[i];
  			if (priv->flags & TC_MQPRIO_F_MAX_RATE)
  				for (i = 0; i < mqprio.qopt.num_tc; i++)
  					mqprio.max_rate[i] = priv->max_rate[i];
  			break;
  		default:
  			return -EINVAL;
  		}
  		err = dev->netdev_ops->ndo_setup_tc(dev,
575ed7d39   Nogah Frankel   net_sch: mqprio: ...
259
  						    TC_SETUP_QDISC_MQPRIO,
de4784ca0   Jiri Pirko   net: sched: get r...
260
  						    &mqprio);
b8970f0bf   John Fastabend   net_sched: implem...
261
  		if (err)
87b60cfac   Eric Dumazet   net_sched: fix er...
262
  			return err;
2026fecf5   Alexander Duyck   mqprio: Change ha...
263

4e8b86c06   Amritha Nambiar   mqprio: Introduce...
264
  		priv->hw_offload = mqprio.qopt.hw;
b8970f0bf   John Fastabend   net_sched: implem...
265
266
267
268
269
270
271
272
273
274
275
276
277
  	} else {
  		netdev_set_num_tc(dev, qopt->num_tc);
  		for (i = 0; i < qopt->num_tc; i++)
  			netdev_set_tc_queue(dev, i,
  					    qopt->count[i], qopt->offset[i]);
  	}
  
  	/* Always use supplied priority mappings */
  	for (i = 0; i < TC_BITMASK + 1; i++)
  		netdev_set_prio_tc_map(dev, i, qopt->prio_tc_map[i]);
  
  	sch->flags |= TCQ_F_MQROOT;
  	return 0;
b8970f0bf   John Fastabend   net_sched: implem...
278
279
280
281
282
283
  }
  
  static void mqprio_attach(struct Qdisc *sch)
  {
  	struct net_device *dev = qdisc_dev(sch);
  	struct mqprio_sched *priv = qdisc_priv(sch);
95dc19299   Eric Dumazet   pkt_sched: give v...
284
  	struct Qdisc *qdisc, *old;
b8970f0bf   John Fastabend   net_sched: implem...
285
286
287
288
289
  	unsigned int ntx;
  
  	/* Attach underlying qdisc */
  	for (ntx = 0; ntx < dev->num_tx_queues; ntx++) {
  		qdisc = priv->qdiscs[ntx];
95dc19299   Eric Dumazet   pkt_sched: give v...
290
291
  		old = dev_graft_qdisc(qdisc->dev_queue, qdisc);
  		if (old)
86bd446b5   Vlad Buslov   net: sched: renam...
292
  			qdisc_put(old);
95dc19299   Eric Dumazet   pkt_sched: give v...
293
  		if (ntx < dev->real_num_tx_queues)
49b499718   Jiri Kosina   net: sched: make ...
294
  			qdisc_hash_add(qdisc, false);
b8970f0bf   John Fastabend   net_sched: implem...
295
296
297
298
299
300
301
302
303
  	}
  	kfree(priv->qdiscs);
  	priv->qdiscs = NULL;
  }
  
  static struct netdev_queue *mqprio_queue_get(struct Qdisc *sch,
  					     unsigned long cl)
  {
  	struct net_device *dev = qdisc_dev(sch);
32302902f   Alexander Duyck   mqprio: Reserve l...
304
  	unsigned long ntx = cl - 1;
b8970f0bf   John Fastabend   net_sched: implem...
305
306
307
308
309
310
311
  
  	if (ntx >= dev->num_tx_queues)
  		return NULL;
  	return netdev_get_tx_queue(dev, ntx);
  }
  
  static int mqprio_graft(struct Qdisc *sch, unsigned long cl, struct Qdisc *new,
653d6fd68   Alexander Aring   net: sched: sch: ...
312
  			struct Qdisc **old, struct netlink_ext_ack *extack)
b8970f0bf   John Fastabend   net_sched: implem...
313
314
315
316
317
318
319
320
321
322
323
  {
  	struct net_device *dev = qdisc_dev(sch);
  	struct netdev_queue *dev_queue = mqprio_queue_get(sch, cl);
  
  	if (!dev_queue)
  		return -EINVAL;
  
  	if (dev->flags & IFF_UP)
  		dev_deactivate(dev);
  
  	*old = dev_graft_qdisc(dev_queue, new);
1abbe1394   Eric Dumazet   pkt_sched: avoid ...
324
  	if (new)
4eaf3b84f   Eric Dumazet   net_sched: fix qd...
325
  		new->flags |= TCQ_F_ONETXQUEUE | TCQ_F_NOPARENT;
1abbe1394   Eric Dumazet   pkt_sched: avoid ...
326

b8970f0bf   John Fastabend   net_sched: implem...
327
328
329
330
331
  	if (dev->flags & IFF_UP)
  		dev_activate(dev);
  
  	return 0;
  }
4e8b86c06   Amritha Nambiar   mqprio: Introduce...
332
333
334
335
336
337
338
  static int dump_rates(struct mqprio_sched *priv,
  		      struct tc_mqprio_qopt *opt, struct sk_buff *skb)
  {
  	struct nlattr *nest;
  	int i;
  
  	if (priv->flags & TC_MQPRIO_F_MIN_RATE) {
ae0be8de9   Michal Kubecek   netlink: make nla...
339
  		nest = nla_nest_start_noflag(skb, TCA_MQPRIO_MIN_RATE64);
4e8b86c06   Amritha Nambiar   mqprio: Introduce...
340
341
342
343
344
345
346
347
348
349
350
351
352
  		if (!nest)
  			goto nla_put_failure;
  
  		for (i = 0; i < opt->num_tc; i++) {
  			if (nla_put(skb, TCA_MQPRIO_MIN_RATE64,
  				    sizeof(priv->min_rate[i]),
  				    &priv->min_rate[i]))
  				goto nla_put_failure;
  		}
  		nla_nest_end(skb, nest);
  	}
  
  	if (priv->flags & TC_MQPRIO_F_MAX_RATE) {
ae0be8de9   Michal Kubecek   netlink: make nla...
353
  		nest = nla_nest_start_noflag(skb, TCA_MQPRIO_MAX_RATE64);
4e8b86c06   Amritha Nambiar   mqprio: Introduce...
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
  		if (!nest)
  			goto nla_put_failure;
  
  		for (i = 0; i < opt->num_tc; i++) {
  			if (nla_put(skb, TCA_MQPRIO_MAX_RATE64,
  				    sizeof(priv->max_rate[i]),
  				    &priv->max_rate[i]))
  				goto nla_put_failure;
  		}
  		nla_nest_end(skb, nest);
  	}
  	return 0;
  
  nla_put_failure:
  	nla_nest_cancel(skb, nest);
  	return -1;
  }
b8970f0bf   John Fastabend   net_sched: implem...
371
372
373
374
  static int mqprio_dump(struct Qdisc *sch, struct sk_buff *skb)
  {
  	struct net_device *dev = qdisc_dev(sch);
  	struct mqprio_sched *priv = qdisc_priv(sch);
4e8b86c06   Amritha Nambiar   mqprio: Introduce...
375
  	struct nlattr *nla = (struct nlattr *)skb_tail_pointer(skb);
144ce879b   Eric Dumazet   net_sched: sch_mq...
376
  	struct tc_mqprio_qopt opt = { 0 };
b8970f0bf   John Fastabend   net_sched: implem...
377
  	struct Qdisc *qdisc;
ce679e8df   John Fastabend   net: sched: add s...
378
  	unsigned int ntx, tc;
b8970f0bf   John Fastabend   net_sched: implem...
379
380
381
382
  
  	sch->q.qlen = 0;
  	memset(&sch->bstats, 0, sizeof(sch->bstats));
  	memset(&sch->qstats, 0, sizeof(sch->qstats));
ce679e8df   John Fastabend   net: sched: add s...
383
384
385
386
387
388
389
  	/* MQ supports lockless qdiscs. However, statistics accounting needs
  	 * to account for all, none, or a mix of locked and unlocked child
  	 * qdiscs. Percpu stats are added to counters in-band and locking
  	 * qdisc totals are added at end.
  	 */
  	for (ntx = 0; ntx < dev->num_tx_queues; ntx++) {
  		qdisc = netdev_get_tx_queue(dev, ntx)->qdisc_sleeping;
b8970f0bf   John Fastabend   net_sched: implem...
390
  		spin_lock_bh(qdisc_lock(qdisc));
ce679e8df   John Fastabend   net: sched: add s...
391
392
393
394
395
396
397
398
399
400
  
  		if (qdisc_is_percpu_stats(qdisc)) {
  			__u32 qlen = qdisc_qlen_sum(qdisc);
  
  			__gnet_stats_copy_basic(NULL, &sch->bstats,
  						qdisc->cpu_bstats,
  						&qdisc->bstats);
  			__gnet_stats_copy_queue(&sch->qstats,
  						qdisc->cpu_qstats,
  						&qdisc->qstats, qlen);
2f23cd42e   Dust Li   net: sched: fix d...
401
  			sch->q.qlen		+= qlen;
ce679e8df   John Fastabend   net: sched: add s...
402
403
404
405
406
407
408
409
410
  		} else {
  			sch->q.qlen		+= qdisc->q.qlen;
  			sch->bstats.bytes	+= qdisc->bstats.bytes;
  			sch->bstats.packets	+= qdisc->bstats.packets;
  			sch->qstats.backlog	+= qdisc->qstats.backlog;
  			sch->qstats.drops	+= qdisc->qstats.drops;
  			sch->qstats.requeues	+= qdisc->qstats.requeues;
  			sch->qstats.overlimits	+= qdisc->qstats.overlimits;
  		}
b8970f0bf   John Fastabend   net_sched: implem...
411
412
413
414
415
  		spin_unlock_bh(qdisc_lock(qdisc));
  	}
  
  	opt.num_tc = netdev_get_num_tc(dev);
  	memcpy(opt.prio_tc_map, dev->prio_tc_map, sizeof(opt.prio_tc_map));
2026fecf5   Alexander Duyck   mqprio: Change ha...
416
  	opt.hw = priv->hw_offload;
b8970f0bf   John Fastabend   net_sched: implem...
417

ce679e8df   John Fastabend   net: sched: add s...
418
419
420
  	for (tc = 0; tc < netdev_get_num_tc(dev); tc++) {
  		opt.count[tc] = dev->tc_to_txq[tc].count;
  		opt.offset[tc] = dev->tc_to_txq[tc].offset;
b8970f0bf   John Fastabend   net_sched: implem...
421
  	}
9f104c773   Vladyslav Tarasiuk   mqprio: Fix out-o...
422
  	if (nla_put(skb, TCA_OPTIONS, sizeof(opt), &opt))
4e8b86c06   Amritha Nambiar   mqprio: Introduce...
423
424
425
426
427
428
429
430
431
432
433
434
435
  		goto nla_put_failure;
  
  	if ((priv->flags & TC_MQPRIO_F_MODE) &&
  	    nla_put_u16(skb, TCA_MQPRIO_MODE, priv->mode))
  		goto nla_put_failure;
  
  	if ((priv->flags & TC_MQPRIO_F_SHAPER) &&
  	    nla_put_u16(skb, TCA_MQPRIO_SHAPER, priv->shaper))
  		goto nla_put_failure;
  
  	if ((priv->flags & TC_MQPRIO_F_MIN_RATE ||
  	     priv->flags & TC_MQPRIO_F_MAX_RATE) &&
  	    (dump_rates(priv, &opt, skb) != 0))
1b34ec43c   David S. Miller   pkt_sched: Stop u...
436
  		goto nla_put_failure;
b8970f0bf   John Fastabend   net_sched: implem...
437

4e8b86c06   Amritha Nambiar   mqprio: Introduce...
438
  	return nla_nest_end(skb, nla);
b8970f0bf   John Fastabend   net_sched: implem...
439
  nla_put_failure:
4e8b86c06   Amritha Nambiar   mqprio: Introduce...
440
  	nlmsg_trim(skb, nla);
b8970f0bf   John Fastabend   net_sched: implem...
441
442
443
444
445
446
447
448
449
450
451
452
  	return -1;
  }
  
  static struct Qdisc *mqprio_leaf(struct Qdisc *sch, unsigned long cl)
  {
  	struct netdev_queue *dev_queue = mqprio_queue_get(sch, cl);
  
  	if (!dev_queue)
  		return NULL;
  
  	return dev_queue->qdisc_sleeping;
  }
143976ce9   WANG Cong   net_sched: remove...
453
  static unsigned long mqprio_find(struct Qdisc *sch, u32 classid)
b8970f0bf   John Fastabend   net_sched: implem...
454
455
456
  {
  	struct net_device *dev = qdisc_dev(sch);
  	unsigned int ntx = TC_H_MIN(classid);
32302902f   Alexander Duyck   mqprio: Reserve l...
457
458
459
460
461
462
463
464
465
466
467
468
  	/* There are essentially two regions here that have valid classid
  	 * values. The first region will have a classid value of 1 through
  	 * num_tx_queues. All of these are backed by actual Qdiscs.
  	 */
  	if (ntx < TC_H_MIN_PRIORITY)
  		return (ntx <= dev->num_tx_queues) ? ntx : 0;
  
  	/* The second region represents the hardware traffic classes. These
  	 * are represented by classid values of TC_H_MIN_PRIORITY through
  	 * TC_H_MIN_PRIORITY + netdev_get_num_tc - 1
  	 */
  	return ((ntx - TC_H_MIN_PRIORITY) < netdev_get_num_tc(dev)) ? ntx : 0;
b8970f0bf   John Fastabend   net_sched: implem...
469
  }
b8970f0bf   John Fastabend   net_sched: implem...
470
471
472
  static int mqprio_dump_class(struct Qdisc *sch, unsigned long cl,
  			 struct sk_buff *skb, struct tcmsg *tcm)
  {
32302902f   Alexander Duyck   mqprio: Reserve l...
473
474
475
476
  	if (cl < TC_H_MIN_PRIORITY) {
  		struct netdev_queue *dev_queue = mqprio_queue_get(sch, cl);
  		struct net_device *dev = qdisc_dev(sch);
  		int tc = netdev_txq_to_tc(dev, cl - 1);
b8970f0bf   John Fastabend   net_sched: implem...
477

32302902f   Alexander Duyck   mqprio: Reserve l...
478
479
480
481
482
  		tcm->tcm_parent = (tc < 0) ? 0 :
  			TC_H_MAKE(TC_H_MAJ(sch->handle),
  				  TC_H_MIN(tc + TC_H_MIN_PRIORITY));
  		tcm->tcm_info = dev_queue->qdisc_sleeping->handle;
  	} else {
b8970f0bf   John Fastabend   net_sched: implem...
483
484
  		tcm->tcm_parent = TC_H_ROOT;
  		tcm->tcm_info = 0;
b8970f0bf   John Fastabend   net_sched: implem...
485
486
487
488
489
490
  	}
  	tcm->tcm_handle |= TC_H_MIN(cl);
  	return 0;
  }
  
  static int mqprio_dump_class_stats(struct Qdisc *sch, unsigned long cl,
ea18fd950   stephen hemminger   mqprio: cleanups
491
492
493
  				   struct gnet_dump *d)
  	__releases(d->lock)
  	__acquires(d->lock)
b8970f0bf   John Fastabend   net_sched: implem...
494
  {
32302902f   Alexander Duyck   mqprio: Reserve l...
495
  	if (cl >= TC_H_MIN_PRIORITY) {
b8970f0bf   John Fastabend   net_sched: implem...
496
  		int i;
640158536   John Fastabend   net: sched: restr...
497
  		__u32 qlen = 0;
b8970f0bf   John Fastabend   net_sched: implem...
498
499
  		struct gnet_stats_queue qstats = {0};
  		struct gnet_stats_basic_packed bstats = {0};
32302902f   Alexander Duyck   mqprio: Reserve l...
500
501
  		struct net_device *dev = qdisc_dev(sch);
  		struct netdev_tc_txq tc = dev->tc_to_txq[cl & TC_BITMASK];
b8970f0bf   John Fastabend   net_sched: implem...
502
503
504
505
506
507
  
  		/* Drop lock here it will be reclaimed before touching
  		 * statistics this is required because the d->lock we
  		 * hold here is the look on dev_queue->qdisc_sleeping
  		 * also acquired below.
  		 */
edb09eb17   Eric Dumazet   net: sched: do no...
508
509
  		if (d->lock)
  			spin_unlock_bh(d->lock);
b8970f0bf   John Fastabend   net_sched: implem...
510
511
  
  		for (i = tc.offset; i < tc.offset + tc.count; i++) {
46e5da40a   John Fastabend   net: qdisc: use r...
512
  			struct netdev_queue *q = netdev_get_tx_queue(dev, i);
ce679e8df   John Fastabend   net: sched: add s...
513
514
515
  			struct Qdisc *qdisc = rtnl_dereference(q->qdisc);
  			struct gnet_stats_basic_cpu __percpu *cpu_bstats = NULL;
  			struct gnet_stats_queue __percpu *cpu_qstats = NULL;
46e5da40a   John Fastabend   net: qdisc: use r...
516

b8970f0bf   John Fastabend   net_sched: implem...
517
  			spin_lock_bh(qdisc_lock(qdisc));
ce679e8df   John Fastabend   net: sched: add s...
518
519
520
521
522
523
524
525
526
527
528
529
  			if (qdisc_is_percpu_stats(qdisc)) {
  				cpu_bstats = qdisc->cpu_bstats;
  				cpu_qstats = qdisc->cpu_qstats;
  			}
  
  			qlen = qdisc_qlen_sum(qdisc);
  			__gnet_stats_copy_basic(NULL, &sch->bstats,
  						cpu_bstats, &qdisc->bstats);
  			__gnet_stats_copy_queue(&sch->qstats,
  						cpu_qstats,
  						&qdisc->qstats,
  						qlen);
b8970f0bf   John Fastabend   net_sched: implem...
530
531
  			spin_unlock_bh(qdisc_lock(qdisc));
  		}
ce679e8df   John Fastabend   net: sched: add s...
532

b8970f0bf   John Fastabend   net_sched: implem...
533
  		/* Reclaim root sleeping lock before completing stats */
edb09eb17   Eric Dumazet   net: sched: do no...
534
535
536
  		if (d->lock)
  			spin_lock_bh(d->lock);
  		if (gnet_stats_copy_basic(NULL, d, NULL, &bstats) < 0 ||
b0ab6f927   John Fastabend   net: sched: enabl...
537
  		    gnet_stats_copy_queue(d, NULL, &qstats, qlen) < 0)
b8970f0bf   John Fastabend   net_sched: implem...
538
539
540
541
542
  			return -1;
  	} else {
  		struct netdev_queue *dev_queue = mqprio_queue_get(sch, cl);
  
  		sch = dev_queue->qdisc_sleeping;
14e54ab91   Dust Li   net: sched: fix `...
543
544
  		if (gnet_stats_copy_basic(qdisc_root_sleeping_running(sch), d,
  					  sch->cpu_bstats, &sch->bstats) < 0 ||
5dd431b6b   Paolo Abeni   net: sched: intro...
545
  		    qdisc_qstats_copy(d, sch) < 0)
b8970f0bf   John Fastabend   net_sched: implem...
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
  			return -1;
  	}
  	return 0;
  }
  
  static void mqprio_walk(struct Qdisc *sch, struct qdisc_walker *arg)
  {
  	struct net_device *dev = qdisc_dev(sch);
  	unsigned long ntx;
  
  	if (arg->stop)
  		return;
  
  	/* Walk hierarchy with a virtual class per tc */
  	arg->count = arg->skip;
32302902f   Alexander Duyck   mqprio: Reserve l...
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
  	for (ntx = arg->skip; ntx < netdev_get_num_tc(dev); ntx++) {
  		if (arg->fn(sch, ntx + TC_H_MIN_PRIORITY, arg) < 0) {
  			arg->stop = 1;
  			return;
  		}
  		arg->count++;
  	}
  
  	/* Pad the values and skip over unused traffic classes */
  	if (ntx < TC_MAX_QUEUE) {
  		arg->count = TC_MAX_QUEUE;
  		ntx = TC_MAX_QUEUE;
  	}
  
  	/* Reset offset, sort out remaining per-queue qdiscs */
  	for (ntx -= TC_MAX_QUEUE; ntx < dev->num_tx_queues; ntx++) {
b8970f0bf   John Fastabend   net_sched: implem...
577
578
  		if (arg->fn(sch, ntx + 1, arg) < 0) {
  			arg->stop = 1;
32302902f   Alexander Duyck   mqprio: Reserve l...
579
  			return;
b8970f0bf   John Fastabend   net_sched: implem...
580
581
582
583
  		}
  		arg->count++;
  	}
  }
0f7787b41   Jesus Sanchez-Palencia   net/sched: Add se...
584
585
586
587
588
  static struct netdev_queue *mqprio_select_queue(struct Qdisc *sch,
  						struct tcmsg *tcm)
  {
  	return mqprio_queue_get(sch, TC_H_MIN(tcm->tcm_parent));
  }
b8970f0bf   John Fastabend   net_sched: implem...
589
590
591
  static const struct Qdisc_class_ops mqprio_class_ops = {
  	.graft		= mqprio_graft,
  	.leaf		= mqprio_leaf,
143976ce9   WANG Cong   net_sched: remove...
592
  	.find		= mqprio_find,
b8970f0bf   John Fastabend   net_sched: implem...
593
594
595
  	.walk		= mqprio_walk,
  	.dump		= mqprio_dump_class,
  	.dump_stats	= mqprio_dump_class_stats,
0f7787b41   Jesus Sanchez-Palencia   net/sched: Add se...
596
  	.select_queue	= mqprio_select_queue,
b8970f0bf   John Fastabend   net_sched: implem...
597
  };
ea18fd950   stephen hemminger   mqprio: cleanups
598
  static struct Qdisc_ops mqprio_qdisc_ops __read_mostly = {
b8970f0bf   John Fastabend   net_sched: implem...
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
  	.cl_ops		= &mqprio_class_ops,
  	.id		= "mqprio",
  	.priv_size	= sizeof(struct mqprio_sched),
  	.init		= mqprio_init,
  	.destroy	= mqprio_destroy,
  	.attach		= mqprio_attach,
  	.dump		= mqprio_dump,
  	.owner		= THIS_MODULE,
  };
  
  static int __init mqprio_module_init(void)
  {
  	return register_qdisc(&mqprio_qdisc_ops);
  }
  
  static void __exit mqprio_module_exit(void)
  {
  	unregister_qdisc(&mqprio_qdisc_ops);
  }
  
  module_init(mqprio_module_init);
  module_exit(mqprio_module_exit);
  
  MODULE_LICENSE("GPL");