[linux-block.git] / net / sched / sch_mq.c

// SPDX-License-Identifier: GPL-2.0-only
/*
 * net/sched/sch_mq.c		Classful multiqueue dummy scheduler
 *
 * Copyright (c) 2009 Patrick McHardy <kaber@trash.net>
 */

#include <linux/types.h>
#include <linux/slab.h>
#include <linux/kernel.h>
#include <linux/export.h>
#include <linux/string.h>
#include <linux/errno.h>
#include <linux/skbuff.h>
#include <net/netlink.h>
#include <net/pkt_cls.h>
#include <net/pkt_sched.h>
#include <net/sch_generic.h>

struct mq_sched {
	struct Qdisc		**qdiscs;
};

static int mq_offload(struct Qdisc *sch, enum tc_mq_command cmd)
{
	struct net_device *dev = qdisc_dev(sch);
	struct tc_mq_qopt_offload opt = {
		.command = cmd,
		.handle = sch->handle,
	};

	if (!tc_can_offload(dev) || !dev->netdev_ops->ndo_setup_tc)
		return -EOPNOTSUPP;

	return dev->netdev_ops->ndo_setup_tc(dev, TC_SETUP_QDISC_MQ, &opt);
}

static int mq_offload_stats(struct Qdisc *sch)
{
	struct tc_mq_qopt_offload opt = {
		.command = TC_MQ_STATS,
		.handle = sch->handle,
		.stats = {
			.bstats = &sch->bstats,
			.qstats = &sch->qstats,
		},
	};

	return qdisc_offload_dump_helper(sch, TC_SETUP_QDISC_MQ, &opt);
}

static void mq_destroy(struct Qdisc *sch)
{
	struct net_device *dev = qdisc_dev(sch);
	struct mq_sched *priv = qdisc_priv(sch);
	unsigned int ntx;

	mq_offload(sch, TC_MQ_DESTROY);

	if (!priv->qdiscs)
		return;
	for (ntx = 0; ntx < dev->num_tx_queues && priv->qdiscs[ntx]; ntx++)
		qdisc_put(priv->qdiscs[ntx]);
	kfree(priv->qdiscs);
}

static int mq_init(struct Qdisc *sch, struct nlattr *opt,
		   struct netlink_ext_ack *extack)
{
	struct net_device *dev = qdisc_dev(sch);
	struct mq_sched *priv = qdisc_priv(sch);
	struct netdev_queue *dev_queue;
	struct Qdisc *qdisc;
	unsigned int ntx;

	if (sch->parent != TC_H_ROOT)
		return -EOPNOTSUPP;

	if (!netif_is_multiqueue(dev))
		return -EOPNOTSUPP;

	/* pre-allocate qdiscs, attachment can't fail */
	priv->qdiscs = kcalloc(dev->num_tx_queues, sizeof(priv->qdiscs[0]),
			       GFP_KERNEL);
	if (!priv->qdiscs)
		return -ENOMEM;

	for (ntx = 0; ntx < dev->num_tx_queues; ntx++) {
		dev_queue = netdev_get_tx_queue(dev, ntx);
		qdisc = qdisc_create_dflt(dev_queue, get_default_qdisc_ops(dev, ntx),
					  TC_H_MAKE(TC_H_MAJ(sch->handle),
						    TC_H_MIN(ntx + 1)),
					  extack);
		if (!qdisc)
			return -ENOMEM;
		priv->qdiscs[ntx] = qdisc;
		qdisc->flags |= TCQ_F_ONETXQUEUE | TCQ_F_NOPARENT;
	}

	sch->flags |= TCQ_F_MQROOT;

	mq_offload(sch, TC_MQ_CREATE);
	return 0;
}

static void mq_attach(struct Qdisc *sch)
{
	struct net_device *dev = qdisc_dev(sch);
	struct mq_sched *priv = qdisc_priv(sch);
	struct Qdisc *qdisc, *old;
	unsigned int ntx;

	for (ntx = 0; ntx < dev->num_tx_queues; ntx++) {
		qdisc = priv->qdiscs[ntx];
		old = dev_graft_qdisc(qdisc->dev_queue, qdisc);
		if (old)
			qdisc_put(old);
#ifdef CONFIG_NET_SCHED
		if (ntx < dev->real_num_tx_queues)
			qdisc_hash_add(qdisc, false);
#endif

	}
	kfree(priv->qdiscs);
	priv->qdiscs = NULL;
}

static int mq_dump(struct Qdisc *sch, struct sk_buff *skb)
{
	struct net_device *dev = qdisc_dev(sch);
	struct Qdisc *qdisc;
	unsigned int ntx;
	__u32 qlen = 0;

	sch->q.qlen = 0;
	memset(&sch->bstats, 0, sizeof(sch->bstats));
	memset(&sch->qstats, 0, sizeof(sch->qstats));

	/* 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;
		spin_lock_bh(qdisc_lock(qdisc));

		if (qdisc_is_percpu_stats(qdisc)) {
			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);
			sch->q.qlen		+= qlen;
		} else {
			sch->q.qlen		+= qdisc->q.qlen;
			sch->bstats.bytes	+= qdisc->bstats.bytes;
			sch->bstats.packets	+= qdisc->bstats.packets;
			sch->qstats.qlen	+= qdisc->qstats.qlen;
			sch->qstats.backlog	+= qdisc->qstats.backlog;
			sch->qstats.drops	+= qdisc->qstats.drops;
			sch->qstats.requeues	+= qdisc->qstats.requeues;
			sch->qstats.overlimits	+= qdisc->qstats.overlimits;
		}

		spin_unlock_bh(qdisc_lock(qdisc));
	}

	return mq_offload_stats(sch);
}

static struct netdev_queue *mq_queue_get(struct Qdisc *sch, unsigned long cl)
{
	struct net_device *dev = qdisc_dev(sch);
	unsigned long ntx = cl - 1;

	if (ntx >= dev->num_tx_queues)
		return NULL;
	return netdev_get_tx_queue(dev, ntx);
}

static struct netdev_queue *mq_select_queue(struct Qdisc *sch,
					    struct tcmsg *tcm)
{
	return mq_queue_get(sch, TC_H_MIN(tcm->tcm_parent));
}

static int mq_graft(struct Qdisc *sch, unsigned long cl, struct Qdisc *new,
		    struct Qdisc **old, struct netlink_ext_ack *extack)
{
	struct netdev_queue *dev_queue = mq_queue_get(sch, cl);
	struct tc_mq_qopt_offload graft_offload;
	struct net_device *dev = qdisc_dev(sch);

	if (dev->flags & IFF_UP)
		dev_deactivate(dev);

	*old = dev_graft_qdisc(dev_queue, new);
	if (new)
		new->flags |= TCQ_F_ONETXQUEUE | TCQ_F_NOPARENT;
	if (dev->flags & IFF_UP)
		dev_activate(dev);

	graft_offload.handle = sch->handle;
	graft_offload.graft_params.queue = cl - 1;
	graft_offload.graft_params.child_handle = new ? new->handle : 0;
	graft_offload.command = TC_MQ_GRAFT;

	qdisc_offload_graft_helper(qdisc_dev(sch), sch, new, *old,
				   TC_SETUP_QDISC_MQ, &graft_offload, extack);
	return 0;
}

static struct Qdisc *mq_leaf(struct Qdisc *sch, unsigned long cl)
{
	struct netdev_queue *dev_queue = mq_queue_get(sch, cl);

	return dev_queue->qdisc_sleeping;
}

static unsigned long mq_find(struct Qdisc *sch, u32 classid)
{
	unsigned int ntx = TC_H_MIN(classid);

	if (!mq_queue_get(sch, ntx))
		return 0;
	return ntx;
}

static int mq_dump_class(struct Qdisc *sch, unsigned long cl,
			 struct sk_buff *skb, struct tcmsg *tcm)
{
	struct netdev_queue *dev_queue = mq_queue_get(sch, cl);

	tcm->tcm_parent = TC_H_ROOT;
	tcm->tcm_handle |= TC_H_MIN(cl);
	tcm->tcm_info = dev_queue->qdisc_sleeping->handle;
	return 0;
}

static int mq_dump_class_stats(struct Qdisc *sch, unsigned long cl,
			       struct gnet_dump *d)
{
	struct netdev_queue *dev_queue = mq_queue_get(sch, cl);

	sch = dev_queue->qdisc_sleeping;
	if (gnet_stats_copy_basic(&sch->running, d, sch->cpu_bstats,
				  &sch->bstats) < 0 ||
	    qdisc_qstats_copy(d, sch) < 0)
		return -1;
	return 0;
}

static void mq_walk(struct Qdisc *sch, struct qdisc_walker *arg)
{
	struct net_device *dev = qdisc_dev(sch);
	unsigned int ntx;

	if (arg->stop)
		return;

	arg->count = arg->skip;
	for (ntx = arg->skip; ntx < dev->num_tx_queues; ntx++) {
		if (arg->fn(sch, ntx + 1, arg) < 0) {
			arg->stop = 1;
			break;
		}
		arg->count++;
	}
}

static const struct Qdisc_class_ops mq_class_ops = {
	.select_queue	= mq_select_queue,
	.graft		= mq_graft,
	.leaf		= mq_leaf,
	.find		= mq_find,
	.walk		= mq_walk,
	.dump		= mq_dump_class,
	.dump_stats	= mq_dump_class_stats,
};

struct Qdisc_ops mq_qdisc_ops __read_mostly = {
	.cl_ops		= &mq_class_ops,
	.id		= "mq",
	.priv_size	= sizeof(struct mq_sched),
	.init		= mq_init,
	.destroy	= mq_destroy,
	.attach		= mq_attach,
	.dump		= mq_dump,
	.owner		= THIS_MODULE,
};
Commit	Line	Data
d2912cb1	1	// SPDX-License-Identifier: GPL-2.0-only
6ec1c69a DM	2	/*
	3	* net/sched/sch_mq.c Classful multiqueue dummy scheduler
	4	*
	5	* Copyright (c) 2009 Patrick McHardy <kaber@trash.net>
6ec1c69a DM	6	*/
	7
	8	#include <linux/types.h>
5a0e3ad6	9	#include <linux/slab.h>
6ec1c69a	10	#include <linux/kernel.h>
bc3b2d7f	11	#include <linux/export.h>
6ec1c69a DM	12	#include <linux/string.h>
	13	#include <linux/errno.h>
	14	#include <linux/skbuff.h>
	15	#include <net/netlink.h>
f971b132	16	#include <net/pkt_cls.h>
6ec1c69a	17	#include <net/pkt_sched.h>
b01ac095	18	#include <net/sch_generic.h>
6ec1c69a DM	19
	20	struct mq_sched {
	21	struct Qdisc **qdiscs;
	22	};
	23
f971b132 JK	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
58f89273	38	static int mq_offload_stats(struct Qdisc *sch)
47c669a4	39	{
47c669a4 JK	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
58f89273	49	return qdisc_offload_dump_helper(sch, TC_SETUP_QDISC_MQ, &opt);
47c669a4 JK	50	}
47c669a4 JK	51
6ec1c69a DM	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
f971b132 JK	58	mq_offload(sch, TC_MQ_DESTROY);
f971b132 JK	59
6ec1c69a DM	60	if (!priv->qdiscs)
	61	return;
	62	for (ntx = 0; ntx < dev->num_tx_queues && priv->qdiscs[ntx]; ntx++)
86bd446b	63	qdisc_put(priv->qdiscs[ntx]);
6ec1c69a DM	64	kfree(priv->qdiscs);
	65	}
	66
e63d7dfd AA	67	static int mq_init(struct Qdisc sch, struct nlattr opt,
e63d7dfd AA	68	struct netlink_ext_ack *extack)
6ec1c69a DM	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);
87b60cfa	85	if (!priv->qdiscs)
6ec1c69a DM	86	return -ENOMEM;
	87
	88	for (ntx = 0; ntx < dev->num_tx_queues; ntx++) {
	89	dev_queue = netdev_get_tx_queue(dev, ntx);
1f27cde3	90	qdisc = qdisc_create_dflt(dev_queue, get_default_qdisc_ops(dev, ntx),
6ec1c69a	91	TC_H_MAKE(TC_H_MAJ(sch->handle),
a38a9882 AA	92	TC_H_MIN(ntx + 1)),
a38a9882 AA	93	extack);
87b60cfa ED	94	if (!qdisc)
87b60cfa ED	95	return -ENOMEM;
6ec1c69a	96	priv->qdiscs[ntx] = qdisc;
4eaf3b84	97	qdisc->flags \|= TCQ_F_ONETXQUEUE \| TCQ_F_NOPARENT;
6ec1c69a DM	98	}
6ec1c69a DM	99
23bcf634	100	sch->flags \|= TCQ_F_MQROOT;
f971b132 JK	101
f971b132 JK	102	mq_offload(sch, TC_MQ_CREATE);
6ec1c69a	103	return 0;
6ec1c69a DM	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);
95dc1929	110	struct Qdisc qdisc, old;
6ec1c69a DM	111	unsigned int ntx;
	112
	113	for (ntx = 0; ntx < dev->num_tx_queues; ntx++) {
	114	qdisc = priv->qdiscs[ntx];
95dc1929 ED	115	old = dev_graft_qdisc(qdisc->dev_queue, qdisc);
95dc1929 ED	116	if (old)
86bd446b	117	qdisc_put(old);
95dc1929 ED	118	#ifdef CONFIG_NET_SCHED
95dc1929 ED	119	if (ntx < dev->real_num_tx_queues)
49b49971	120	qdisc_hash_add(qdisc, false);
95dc1929 ED	121	#endif
95dc1929 ED	122
6ec1c69a DM	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;
ce679e8d	133	__u32 qlen = 0;
6ec1c69a DM	134
	135	sch->q.qlen = 0;
	136	memset(&sch->bstats, 0, sizeof(sch->bstats));
	137	memset(&sch->qstats, 0, sizeof(sch->qstats));
	138
ce679e8d JF	139	/* MQ supports lockless qdiscs. However, statistics accounting needs
	140	* to account for all, none, or a mix of locked and unlocked child
	141	* qdiscs. Percpu stats are added to counters in-band and locking
	142	* qdisc totals are added at end.
	143	*/
6ec1c69a DM	144	for (ntx = 0; ntx < dev->num_tx_queues; ntx++) {
	145	qdisc = netdev_get_tx_queue(dev, ntx)->qdisc_sleeping;
	146	spin_lock_bh(qdisc_lock(qdisc));
b01ac095 JF	147
b01ac095 JF	148	if (qdisc_is_percpu_stats(qdisc)) {
ce679e8d JF	149	qlen = qdisc_qlen_sum(qdisc);
	150	__gnet_stats_copy_basic(NULL, &sch->bstats,
	151	qdisc->cpu_bstats,
	152	&qdisc->bstats);
	153	__gnet_stats_copy_queue(&sch->qstats,
	154	qdisc->cpu_qstats,
	155	&qdisc->qstats, qlen);
2f23cd42	156	sch->q.qlen += qlen;
ce679e8d JF	157	} else {
	158	sch->q.qlen += qdisc->q.qlen;
	159	sch->bstats.bytes += qdisc->bstats.bytes;
	160	sch->bstats.packets += qdisc->bstats.packets;
47c669a4	161	sch->qstats.qlen += qdisc->qstats.qlen;
ce679e8d JF	162	sch->qstats.backlog += qdisc->qstats.backlog;
	163	sch->qstats.drops += qdisc->qstats.drops;
	164	sch->qstats.requeues += qdisc->qstats.requeues;
	165	sch->qstats.overlimits += qdisc->qstats.overlimits;
b01ac095 JF	166	}
b01ac095 JF	167
6ec1c69a DM	168	spin_unlock_bh(qdisc_lock(qdisc));
6ec1c69a DM	169	}
ce679e8d	170
58f89273	171	return mq_offload_stats(sch);
6ec1c69a DM	172	}
	173
	174	static struct netdev_queue mq_queue_get(struct Qdisc sch, unsigned long cl)
	175	{
	176	struct net_device *dev = qdisc_dev(sch);
	177	unsigned long ntx = cl - 1;
	178
	179	if (ntx >= dev->num_tx_queues)
	180	return NULL;
	181	return netdev_get_tx_queue(dev, ntx);
	182	}
	183
926e61b7 JP	184	static struct netdev_queue mq_select_queue(struct Qdisc sch,
926e61b7 JP	185	struct tcmsg *tcm)
6ec1c69a	186	{
ce8a75f6	187	return mq_queue_get(sch, TC_H_MIN(tcm->tcm_parent));
6ec1c69a DM	188	}
	189
	190	static int mq_graft(struct Qdisc sch, unsigned long cl, struct Qdisc new,
653d6fd6	191	struct Qdisc *old, struct netlink_ext_ack extack)
6ec1c69a DM	192	{
6ec1c69a DM	193	struct netdev_queue *dev_queue = mq_queue_get(sch, cl);
d577a3d2	194	struct tc_mq_qopt_offload graft_offload;
6ec1c69a DM	195	struct net_device *dev = qdisc_dev(sch);
	196
	197	if (dev->flags & IFF_UP)
	198	dev_deactivate(dev);
	199
	200	*old = dev_graft_qdisc(dev_queue, new);
1abbe139	201	if (new)
4eaf3b84	202	new->flags \|= TCQ_F_ONETXQUEUE \| TCQ_F_NOPARENT;
6ec1c69a DM	203	if (dev->flags & IFF_UP)
6ec1c69a DM	204	dev_activate(dev);
d577a3d2 JK	205
	206	graft_offload.handle = sch->handle;
	207	graft_offload.graft_params.queue = cl - 1;
	208	graft_offload.graft_params.child_handle = new ? new->handle : 0;
	209	graft_offload.command = TC_MQ_GRAFT;
	210
	211	qdisc_offload_graft_helper(qdisc_dev(sch), sch, new, *old,
	212	TC_SETUP_QDISC_MQ, &graft_offload, extack);
6ec1c69a DM	213	return 0;
	214	}
	215
	216	static struct Qdisc mq_leaf(struct Qdisc sch, unsigned long cl)
	217	{
	218	struct netdev_queue *dev_queue = mq_queue_get(sch, cl);
	219
	220	return dev_queue->qdisc_sleeping;
	221	}
	222
143976ce	223	static unsigned long mq_find(struct Qdisc *sch, u32 classid)
6ec1c69a DM	224	{
	225	unsigned int ntx = TC_H_MIN(classid);
	226
	227	if (!mq_queue_get(sch, ntx))
	228	return 0;
	229	return ntx;
	230	}
	231
6ec1c69a DM	232	static int mq_dump_class(struct Qdisc *sch, unsigned long cl,
	233	struct sk_buff skb, struct tcmsg tcm)
	234	{
	235	struct netdev_queue *dev_queue = mq_queue_get(sch, cl);
	236
	237	tcm->tcm_parent = TC_H_ROOT;
	238	tcm->tcm_handle \|= TC_H_MIN(cl);
	239	tcm->tcm_info = dev_queue->qdisc_sleeping->handle;
	240	return 0;
	241	}
	242
	243	static int mq_dump_class_stats(struct Qdisc *sch, unsigned long cl,
	244	struct gnet_dump *d)
	245	{
	246	struct netdev_queue *dev_queue = mq_queue_get(sch, cl);
	247
	248	sch = dev_queue->qdisc_sleeping;
14e54ab9 DL	249	if (gnet_stats_copy_basic(&sch->running, d, sch->cpu_bstats,
14e54ab9 DL	250	&sch->bstats) < 0 \|\|
5dd431b6	251	qdisc_qstats_copy(d, sch) < 0)
6ec1c69a DM	252	return -1;
	253	return 0;
	254	}
	255
	256	static void mq_walk(struct Qdisc sch, struct qdisc_walker arg)
	257	{
	258	struct net_device *dev = qdisc_dev(sch);
	259	unsigned int ntx;
	260
	261	if (arg->stop)
	262	return;
	263
	264	arg->count = arg->skip;
	265	for (ntx = arg->skip; ntx < dev->num_tx_queues; ntx++) {
	266	if (arg->fn(sch, ntx + 1, arg) < 0) {
	267	arg->stop = 1;
	268	break;
	269	}
	270	arg->count++;
	271	}
	272	}
	273
	274	static const struct Qdisc_class_ops mq_class_ops = {
	275	.select_queue = mq_select_queue,
	276	.graft = mq_graft,
	277	.leaf = mq_leaf,
143976ce	278	.find = mq_find,
6ec1c69a DM	279	.walk = mq_walk,
	280	.dump = mq_dump_class,
	281	.dump_stats = mq_dump_class_stats,
	282	};
	283
	284	struct Qdisc_ops mq_qdisc_ops __read_mostly = {
	285	.cl_ops = &mq_class_ops,
	286	.id = "mq",
	287	.priv_size = sizeof(struct mq_sched),
	288	.init = mq_init,
	289	.destroy = mq_destroy,
	290	.attach = mq_attach,
	291	.dump = mq_dump,
	292	.owner = THIS_MODULE,
	293	};