xref: /openbmc/linux/net/sched/sch_mq.c (revision 7583028d)
1  // SPDX-License-Identifier: GPL-2.0-only
2  /*
3   * net/sched/sch_mq.c		Classful multiqueue dummy scheduler
4   *
5   * Copyright (c) 2009 Patrick McHardy <kaber@trash.net>
6   */
7  
8  #include <linux/types.h>
9  #include <linux/slab.h>
10  #include <linux/kernel.h>
11  #include <linux/export.h>
12  #include <linux/string.h>
13  #include <linux/errno.h>
14  #include <linux/skbuff.h>
15  #include <net/netlink.h>
16  #include <net/pkt_cls.h>
17  #include <net/pkt_sched.h>
18  #include <net/sch_generic.h>
19  
20  struct mq_sched {
21  	struct Qdisc		**qdiscs;
22  };
23  
24  static int mq_offload(struct Qdisc *sch, enum tc_mq_command cmd)
25  {
26  	struct net_device *dev = qdisc_dev(sch);
27  	struct tc_mq_qopt_offload opt = {
28  		.command = cmd,
29  		.handle = sch->handle,
30  	};
31  
32  	if (!tc_can_offload(dev) || !dev->netdev_ops->ndo_setup_tc)
33  		return -EOPNOTSUPP;
34  
35  	return dev->netdev_ops->ndo_setup_tc(dev, TC_SETUP_QDISC_MQ, &opt);
36  }
37  
38  static int mq_offload_stats(struct Qdisc *sch)
39  {
40  	struct tc_mq_qopt_offload opt = {
41  		.command = TC_MQ_STATS,
42  		.handle = sch->handle,
43  		.stats = {
44  			.bstats = &sch->bstats,
45  			.qstats = &sch->qstats,
46  		},
47  	};
48  
49  	return qdisc_offload_dump_helper(sch, TC_SETUP_QDISC_MQ, &opt);
50  }
51  
52  static void mq_destroy(struct Qdisc *sch)
53  {
54  	struct net_device *dev = qdisc_dev(sch);
55  	struct mq_sched *priv = qdisc_priv(sch);
56  	unsigned int ntx;
57  
58  	mq_offload(sch, TC_MQ_DESTROY);
59  
60  	if (!priv->qdiscs)
61  		return;
62  	for (ntx = 0; ntx < dev->num_tx_queues && priv->qdiscs[ntx]; ntx++)
63  		qdisc_put(priv->qdiscs[ntx]);
64  	kfree(priv->qdiscs);
65  }
66  
67  static int mq_init(struct Qdisc *sch, struct nlattr *opt,
68  		   struct netlink_ext_ack *extack)
69  {
70  	struct net_device *dev = qdisc_dev(sch);
71  	struct mq_sched *priv = qdisc_priv(sch);
72  	struct netdev_queue *dev_queue;
73  	struct Qdisc *qdisc;
74  	unsigned int ntx;
75  
76  	if (sch->parent != TC_H_ROOT)
77  		return -EOPNOTSUPP;
78  
79  	if (!netif_is_multiqueue(dev))
80  		return -EOPNOTSUPP;
81  
82  	/* pre-allocate qdiscs, attachment can't fail */
83  	priv->qdiscs = kcalloc(dev->num_tx_queues, sizeof(priv->qdiscs[0]),
84  			       GFP_KERNEL);
85  	if (!priv->qdiscs)
86  		return -ENOMEM;
87  
88  	for (ntx = 0; ntx < dev->num_tx_queues; ntx++) {
89  		dev_queue = netdev_get_tx_queue(dev, ntx);
90  		qdisc = qdisc_create_dflt(dev_queue, get_default_qdisc_ops(dev, ntx),
91  					  TC_H_MAKE(TC_H_MAJ(sch->handle),
92  						    TC_H_MIN(ntx + 1)),
93  					  extack);
94  		if (!qdisc)
95  			return -ENOMEM;
96  		priv->qdiscs[ntx] = qdisc;
97  		qdisc->flags |= TCQ_F_ONETXQUEUE | TCQ_F_NOPARENT;
98  	}
99  
100  	sch->flags |= TCQ_F_MQROOT;
101  
102  	mq_offload(sch, TC_MQ_CREATE);
103  	return 0;
104  }
105  
106  static void mq_attach(struct Qdisc *sch)
107  {
108  	struct net_device *dev = qdisc_dev(sch);
109  	struct mq_sched *priv = qdisc_priv(sch);
110  	struct Qdisc *qdisc, *old;
111  	unsigned int ntx;
112  
113  	for (ntx = 0; ntx < dev->num_tx_queues; ntx++) {
114  		qdisc = priv->qdiscs[ntx];
115  		old = dev_graft_qdisc(qdisc->dev_queue, qdisc);
116  		if (old)
117  			qdisc_put(old);
118  #ifdef CONFIG_NET_SCHED
119  		if (ntx < dev->real_num_tx_queues)
120  			qdisc_hash_add(qdisc, false);
121  #endif
122  
123  	}
124  	kfree(priv->qdiscs);
125  	priv->qdiscs = NULL;
126  }
127  
128  static int mq_dump(struct Qdisc *sch, struct sk_buff *skb)
129  {
130  	struct net_device *dev = qdisc_dev(sch);
131  	struct Qdisc *qdisc;
132  	unsigned int ntx;
133  
134  	sch->q.qlen = 0;
135  	gnet_stats_basic_sync_init(&sch->bstats);
136  	memset(&sch->qstats, 0, sizeof(sch->qstats));
137  
138  	/* MQ supports lockless qdiscs. However, statistics accounting needs
139  	 * to account for all, none, or a mix of locked and unlocked child
140  	 * qdiscs. Percpu stats are added to counters in-band and locking
141  	 * qdisc totals are added at end.
142  	 */
143  	for (ntx = 0; ntx < dev->num_tx_queues; ntx++) {
144  		qdisc = rtnl_dereference(netdev_get_tx_queue(dev, ntx)->qdisc_sleeping);
145  		spin_lock_bh(qdisc_lock(qdisc));
146  
147  		gnet_stats_add_basic(&sch->bstats, qdisc->cpu_bstats,
148  				     &qdisc->bstats, false);
149  		gnet_stats_add_queue(&sch->qstats, qdisc->cpu_qstats,
150  				     &qdisc->qstats);
151  		sch->q.qlen += qdisc_qlen(qdisc);
152  
153  		spin_unlock_bh(qdisc_lock(qdisc));
154  	}
155  
156  	return mq_offload_stats(sch);
157  }
158  
159  static struct netdev_queue *mq_queue_get(struct Qdisc *sch, unsigned long cl)
160  {
161  	struct net_device *dev = qdisc_dev(sch);
162  	unsigned long ntx = cl - 1;
163  
164  	if (ntx >= dev->num_tx_queues)
165  		return NULL;
166  	return netdev_get_tx_queue(dev, ntx);
167  }
168  
169  static struct netdev_queue *mq_select_queue(struct Qdisc *sch,
170  					    struct tcmsg *tcm)
171  {
172  	return mq_queue_get(sch, TC_H_MIN(tcm->tcm_parent));
173  }
174  
175  static int mq_graft(struct Qdisc *sch, unsigned long cl, struct Qdisc *new,
176  		    struct Qdisc **old, struct netlink_ext_ack *extack)
177  {
178  	struct netdev_queue *dev_queue = mq_queue_get(sch, cl);
179  	struct tc_mq_qopt_offload graft_offload;
180  	struct net_device *dev = qdisc_dev(sch);
181  
182  	if (dev->flags & IFF_UP)
183  		dev_deactivate(dev);
184  
185  	*old = dev_graft_qdisc(dev_queue, new);
186  	if (new)
187  		new->flags |= TCQ_F_ONETXQUEUE | TCQ_F_NOPARENT;
188  	if (dev->flags & IFF_UP)
189  		dev_activate(dev);
190  
191  	graft_offload.handle = sch->handle;
192  	graft_offload.graft_params.queue = cl - 1;
193  	graft_offload.graft_params.child_handle = new ? new->handle : 0;
194  	graft_offload.command = TC_MQ_GRAFT;
195  
196  	qdisc_offload_graft_helper(qdisc_dev(sch), sch, new, *old,
197  				   TC_SETUP_QDISC_MQ, &graft_offload, extack);
198  	return 0;
199  }
200  
201  static struct Qdisc *mq_leaf(struct Qdisc *sch, unsigned long cl)
202  {
203  	struct netdev_queue *dev_queue = mq_queue_get(sch, cl);
204  
205  	return rtnl_dereference(dev_queue->qdisc_sleeping);
206  }
207  
208  static unsigned long mq_find(struct Qdisc *sch, u32 classid)
209  {
210  	unsigned int ntx = TC_H_MIN(classid);
211  
212  	if (!mq_queue_get(sch, ntx))
213  		return 0;
214  	return ntx;
215  }
216  
217  static int mq_dump_class(struct Qdisc *sch, unsigned long cl,
218  			 struct sk_buff *skb, struct tcmsg *tcm)
219  {
220  	struct netdev_queue *dev_queue = mq_queue_get(sch, cl);
221  
222  	tcm->tcm_parent = TC_H_ROOT;
223  	tcm->tcm_handle |= TC_H_MIN(cl);
224  	tcm->tcm_info = rtnl_dereference(dev_queue->qdisc_sleeping)->handle;
225  	return 0;
226  }
227  
228  static int mq_dump_class_stats(struct Qdisc *sch, unsigned long cl,
229  			       struct gnet_dump *d)
230  {
231  	struct netdev_queue *dev_queue = mq_queue_get(sch, cl);
232  
233  	sch = rtnl_dereference(dev_queue->qdisc_sleeping);
234  	if (gnet_stats_copy_basic(d, sch->cpu_bstats, &sch->bstats, true) < 0 ||
235  	    qdisc_qstats_copy(d, sch) < 0)
236  		return -1;
237  	return 0;
238  }
239  
240  static void mq_walk(struct Qdisc *sch, struct qdisc_walker *arg)
241  {
242  	struct net_device *dev = qdisc_dev(sch);
243  	unsigned int ntx;
244  
245  	if (arg->stop)
246  		return;
247  
248  	arg->count = arg->skip;
249  	for (ntx = arg->skip; ntx < dev->num_tx_queues; ntx++) {
250  		if (!tc_qdisc_stats_dump(sch, ntx + 1, arg))
251  			break;
252  	}
253  }
254  
255  static const struct Qdisc_class_ops mq_class_ops = {
256  	.select_queue	= mq_select_queue,
257  	.graft		= mq_graft,
258  	.leaf		= mq_leaf,
259  	.find		= mq_find,
260  	.walk		= mq_walk,
261  	.dump		= mq_dump_class,
262  	.dump_stats	= mq_dump_class_stats,
263  };
264  
265  struct Qdisc_ops mq_qdisc_ops __read_mostly = {
266  	.cl_ops		= &mq_class_ops,
267  	.id		= "mq",
268  	.priv_size	= sizeof(struct mq_sched),
269  	.init		= mq_init,
270  	.destroy	= mq_destroy,
271  	.attach		= mq_attach,
272  	.change_real_num_tx = mq_change_real_num_tx,
273  	.dump		= mq_dump,
274  	.owner		= THIS_MODULE,
275  };
276