[linux-2.6-block.git] / net / sched / sch_prio.c

/*
 * net/sched/sch_prio.c	Simple 3-band priority "scheduler".
 *
 *		This program is free software; you can redistribute it and/or
 *		modify it under the terms of the GNU General Public License
 *		as published by the Free Software Foundation; either version
 *		2 of the License, or (at your option) any later version.
 *
 * Authors:	Alexey Kuznetsov, <kuznet@ms2.inr.ac.ru>
 * Fixes:       19990609: J Hadi Salim <hadi@nortelnetworks.com>:
 *              Init --  EINVAL when opt undefined
 */

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

struct prio_sched_data {
	int bands;
	struct tcf_proto __rcu *filter_list;
	struct tcf_block *block;
	u8  prio2band[TC_PRIO_MAX+1];
	struct Qdisc *queues[TCQ_PRIO_BANDS];
};


static struct Qdisc *
prio_classify(struct sk_buff *skb, struct Qdisc *sch, int *qerr)
{
	struct prio_sched_data *q = qdisc_priv(sch);
	u32 band = skb->priority;
	struct tcf_result res;
	struct tcf_proto *fl;
	int err;

	*qerr = NET_XMIT_SUCCESS | __NET_XMIT_BYPASS;
	if (TC_H_MAJ(skb->priority) != sch->handle) {
		fl = rcu_dereference_bh(q->filter_list);
		err = tcf_classify(skb, fl, &res, false);
#ifdef CONFIG_NET_CLS_ACT
		switch (err) {
		case TC_ACT_STOLEN:
		case TC_ACT_QUEUED:
		case TC_ACT_TRAP:
			*qerr = NET_XMIT_SUCCESS | __NET_XMIT_STOLEN;
			/* fall through */
		case TC_ACT_SHOT:
			return NULL;
		}
#endif
		if (!fl || err < 0) {
			if (TC_H_MAJ(band))
				band = 0;
			return q->queues[q->prio2band[band & TC_PRIO_MAX]];
		}
		band = res.classid;
	}
	band = TC_H_MIN(band) - 1;
	if (band >= q->bands)
		return q->queues[q->prio2band[0]];

	return q->queues[band];
}

static int
prio_enqueue(struct sk_buff *skb, struct Qdisc *sch, struct sk_buff **to_free)
{
	struct Qdisc *qdisc;
	int ret;

	qdisc = prio_classify(skb, sch, &ret);
#ifdef CONFIG_NET_CLS_ACT
	if (qdisc == NULL) {

		if (ret & __NET_XMIT_BYPASS)
			qdisc_qstats_drop(sch);
		__qdisc_drop(skb, to_free);
		return ret;
	}
#endif

	ret = qdisc_enqueue(skb, qdisc, to_free);
	if (ret == NET_XMIT_SUCCESS) {
		qdisc_qstats_backlog_inc(sch, skb);
		sch->q.qlen++;
		return NET_XMIT_SUCCESS;
	}
	if (net_xmit_drop_count(ret))
		qdisc_qstats_drop(sch);
	return ret;
}

static struct sk_buff *prio_peek(struct Qdisc *sch)
{
	struct prio_sched_data *q = qdisc_priv(sch);
	int prio;

	for (prio = 0; prio < q->bands; prio++) {
		struct Qdisc *qdisc = q->queues[prio];
		struct sk_buff *skb = qdisc->ops->peek(qdisc);
		if (skb)
			return skb;
	}
	return NULL;
}

static struct sk_buff *prio_dequeue(struct Qdisc *sch)
{
	struct prio_sched_data *q = qdisc_priv(sch);
	int prio;

	for (prio = 0; prio < q->bands; prio++) {
		struct Qdisc *qdisc = q->queues[prio];
		struct sk_buff *skb = qdisc_dequeue_peeked(qdisc);
		if (skb) {
			qdisc_bstats_update(sch, skb);
			qdisc_qstats_backlog_dec(sch, skb);
			sch->q.qlen--;
			return skb;
		}
	}
	return NULL;

}

static void
prio_reset(struct Qdisc *sch)
{
	int prio;
	struct prio_sched_data *q = qdisc_priv(sch);

	for (prio = 0; prio < q->bands; prio++)
		qdisc_reset(q->queues[prio]);
	sch->qstats.backlog = 0;
	sch->q.qlen = 0;
}

static int prio_offload(struct Qdisc *sch, struct tc_prio_qopt *qopt)
{
	struct net_device *dev = qdisc_dev(sch);
	struct tc_prio_qopt_offload opt = {
		.handle = sch->handle,
		.parent = sch->parent,
	};

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

	if (qopt) {
		opt.command = TC_PRIO_REPLACE;
		opt.replace_params.bands = qopt->bands;
		memcpy(&opt.replace_params.priomap, qopt->priomap,
		       TC_PRIO_MAX + 1);
		opt.replace_params.qstats = &sch->qstats;
	} else {
		opt.command = TC_PRIO_DESTROY;
	}

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

static void
prio_destroy(struct Qdisc *sch)
{
	int prio;
	struct prio_sched_data *q = qdisc_priv(sch);

	tcf_block_put(q->block);
	prio_offload(sch, NULL);
	for (prio = 0; prio < q->bands; prio++)
		qdisc_destroy(q->queues[prio]);
}

static int prio_tune(struct Qdisc *sch, struct nlattr *opt,
		     struct netlink_ext_ack *extack)
{
	struct prio_sched_data *q = qdisc_priv(sch);
	struct Qdisc *queues[TCQ_PRIO_BANDS];
	int oldbands = q->bands, i;
	struct tc_prio_qopt *qopt;

	if (nla_len(opt) < sizeof(*qopt))
		return -EINVAL;
	qopt = nla_data(opt);

	if (qopt->bands > TCQ_PRIO_BANDS || qopt->bands < 2)
		return -EINVAL;

	for (i = 0; i <= TC_PRIO_MAX; i++) {
		if (qopt->priomap[i] >= qopt->bands)
			return -EINVAL;
	}

	/* Before commit, make sure we can allocate all new qdiscs */
	for (i = oldbands; i < qopt->bands; i++) {
		queues[i] = qdisc_create_dflt(sch->dev_queue, &pfifo_qdisc_ops,
					      TC_H_MAKE(sch->handle, i + 1),
					      extack);
		if (!queues[i]) {
			while (i > oldbands)
				qdisc_destroy(queues[--i]);
			return -ENOMEM;
		}
	}

	prio_offload(sch, qopt);
	sch_tree_lock(sch);
	q->bands = qopt->bands;
	memcpy(q->prio2band, qopt->priomap, TC_PRIO_MAX+1);

	for (i = q->bands; i < oldbands; i++) {
		struct Qdisc *child = q->queues[i];

		qdisc_tree_reduce_backlog(child, child->q.qlen,
					  child->qstats.backlog);
		qdisc_destroy(child);
	}

	for (i = oldbands; i < q->bands; i++) {
		q->queues[i] = queues[i];
		if (q->queues[i] != &noop_qdisc)
			qdisc_hash_add(q->queues[i], true);
	}

	sch_tree_unlock(sch);
	return 0;
}

static int prio_init(struct Qdisc *sch, struct nlattr *opt,
		     struct netlink_ext_ack *extack)
{
	struct prio_sched_data *q = qdisc_priv(sch);
	int err;

	if (!opt)
		return -EINVAL;

	err = tcf_block_get(&q->block, &q->filter_list, sch, extack);
	if (err)
		return err;

	return prio_tune(sch, opt, extack);
}

static int prio_dump_offload(struct Qdisc *sch)
{
	struct net_device *dev = qdisc_dev(sch);
	struct tc_prio_qopt_offload hw_stats = {
		.command = TC_PRIO_STATS,
		.handle = sch->handle,
		.parent = sch->parent,
		{
			.stats = {
				.bstats = &sch->bstats,
				.qstats = &sch->qstats,
			},
		},
	};
	int err;

	sch->flags &= ~TCQ_F_OFFLOADED;
	if (!tc_can_offload(dev) || !dev->netdev_ops->ndo_setup_tc)
		return 0;

	err = dev->netdev_ops->ndo_setup_tc(dev, TC_SETUP_QDISC_PRIO,
					    &hw_stats);
	if (err == -EOPNOTSUPP)
		return 0;

	if (!err)
		sch->flags |= TCQ_F_OFFLOADED;

	return err;
}

static int prio_dump(struct Qdisc *sch, struct sk_buff *skb)
{
	struct prio_sched_data *q = qdisc_priv(sch);
	unsigned char *b = skb_tail_pointer(skb);
	struct tc_prio_qopt opt;
	int err;

	opt.bands = q->bands;
	memcpy(&opt.priomap, q->prio2band, TC_PRIO_MAX + 1);

	err = prio_dump_offload(sch);
	if (err)
		goto nla_put_failure;

	if (nla_put(skb, TCA_OPTIONS, sizeof(opt), &opt))
		goto nla_put_failure;

	return skb->len;

nla_put_failure:
	nlmsg_trim(skb, b);
	return -1;
}

static int prio_graft(struct Qdisc *sch, unsigned long arg, struct Qdisc *new,
		      struct Qdisc **old, struct netlink_ext_ack *extack)
{
	struct prio_sched_data *q = qdisc_priv(sch);
	struct tc_prio_qopt_offload graft_offload;
	struct net_device *dev = qdisc_dev(sch);
	unsigned long band = arg - 1;
	bool any_qdisc_is_offloaded;
	int err;

	if (new == NULL)
		new = &noop_qdisc;

	*old = qdisc_replace(sch, new, &q->queues[band]);

	if (!tc_can_offload(dev))
		return 0;

	graft_offload.handle = sch->handle;
	graft_offload.parent = sch->parent;
	graft_offload.graft_params.band = band;
	graft_offload.graft_params.child_handle = new->handle;
	graft_offload.command = TC_PRIO_GRAFT;

	err = dev->netdev_ops->ndo_setup_tc(dev, TC_SETUP_QDISC_PRIO,
					    &graft_offload);

	/* Don't report error if the graft is part of destroy operation. */
	if (err && new != &noop_qdisc) {
		/* Don't report error if the parent, the old child and the new
		 * one are not offloaded.
		 */
		any_qdisc_is_offloaded = sch->flags & TCQ_F_OFFLOADED;
		any_qdisc_is_offloaded |= new->flags & TCQ_F_OFFLOADED;
		if (*old)
			any_qdisc_is_offloaded |= (*old)->flags &
						   TCQ_F_OFFLOADED;

		if (any_qdisc_is_offloaded)
			NL_SET_ERR_MSG(extack, "Offloading graft operation failed.");
	}

	return 0;
}

static struct Qdisc *
prio_leaf(struct Qdisc *sch, unsigned long arg)
{
	struct prio_sched_data *q = qdisc_priv(sch);
	unsigned long band = arg - 1;

	return q->queues[band];
}

static unsigned long prio_find(struct Qdisc *sch, u32 classid)
{
	struct prio_sched_data *q = qdisc_priv(sch);
	unsigned long band = TC_H_MIN(classid);

	if (band - 1 >= q->bands)
		return 0;
	return band;
}

static unsigned long prio_bind(struct Qdisc *sch, unsigned long parent, u32 classid)
{
	return prio_find(sch, classid);
}


static void prio_unbind(struct Qdisc *q, unsigned long cl)
{
}

static int prio_dump_class(struct Qdisc *sch, unsigned long cl, struct sk_buff *skb,
			   struct tcmsg *tcm)
{
	struct prio_sched_data *q = qdisc_priv(sch);

	tcm->tcm_handle |= TC_H_MIN(cl);
	tcm->tcm_info = q->queues[cl-1]->handle;
	return 0;
}

static int prio_dump_class_stats(struct Qdisc *sch, unsigned long cl,
				 struct gnet_dump *d)
{
	struct prio_sched_data *q = qdisc_priv(sch);
	struct Qdisc *cl_q;

	cl_q = q->queues[cl - 1];
	if (gnet_stats_copy_basic(qdisc_root_sleeping_running(sch),
				  d, NULL, &cl_q->bstats) < 0 ||
	    gnet_stats_copy_queue(d, NULL, &cl_q->qstats, cl_q->q.qlen) < 0)
		return -1;

	return 0;
}

static void prio_walk(struct Qdisc *sch, struct qdisc_walker *arg)
{
	struct prio_sched_data *q = qdisc_priv(sch);
	int prio;

	if (arg->stop)
		return;

	for (prio = 0; prio < q->bands; prio++) {
		if (arg->count < arg->skip) {
			arg->count++;
			continue;
		}
		if (arg->fn(sch, prio + 1, arg) < 0) {
			arg->stop = 1;
			break;
		}
		arg->count++;
	}
}

static struct tcf_block *prio_tcf_block(struct Qdisc *sch, unsigned long cl,
					struct netlink_ext_ack *extack)
{
	struct prio_sched_data *q = qdisc_priv(sch);

	if (cl)
		return NULL;
	return q->block;
}

static const struct Qdisc_class_ops prio_class_ops = {
	.graft		=	prio_graft,
	.leaf		=	prio_leaf,
	.find		=	prio_find,
	.walk		=	prio_walk,
	.tcf_block	=	prio_tcf_block,
	.bind_tcf	=	prio_bind,
	.unbind_tcf	=	prio_unbind,
	.dump		=	prio_dump_class,
	.dump_stats	=	prio_dump_class_stats,
};

static struct Qdisc_ops prio_qdisc_ops __read_mostly = {
	.next		=	NULL,
	.cl_ops		=	&prio_class_ops,
	.id		=	"prio",
	.priv_size	=	sizeof(struct prio_sched_data),
	.enqueue	=	prio_enqueue,
	.dequeue	=	prio_dequeue,
	.peek		=	prio_peek,
	.init		=	prio_init,
	.reset		=	prio_reset,
	.destroy	=	prio_destroy,
	.change		=	prio_tune,
	.dump		=	prio_dump,
	.owner		=	THIS_MODULE,
};

static int __init prio_module_init(void)
{
	return register_qdisc(&prio_qdisc_ops);
}

static void __exit prio_module_exit(void)
{
	unregister_qdisc(&prio_qdisc_ops);
}

module_init(prio_module_init)
module_exit(prio_module_exit)

MODULE_LICENSE("GPL");
Commit	Line	Data
1da177e4 LT	1	/*
	2	* net/sched/sch_prio.c Simple 3-band priority "scheduler".
	3	*
	4	* This program is free software; you can redistribute it and/or
	5	* modify it under the terms of the GNU General Public License
	6	* as published by the Free Software Foundation; either version
	7	* 2 of the License, or (at your option) any later version.
	8	*
	9	* Authors: Alexey Kuznetsov, <kuznet@ms2.inr.ac.ru>
10297b99	10	* Fixes: 19990609: J Hadi Salim <hadi@nortelnetworks.com>:
1da177e4 LT	11	* Init -- EINVAL when opt undefined
	12	*/
	13
1da177e4	14	#include <linux/module.h>
5a0e3ad6	15	#include <linux/slab.h>
1da177e4 LT	16	#include <linux/types.h>
1da177e4 LT	17	#include <linux/kernel.h>
1da177e4	18	#include <linux/string.h>
1da177e4	19	#include <linux/errno.h>
1da177e4	20	#include <linux/skbuff.h>
dc5fc579	21	#include <net/netlink.h>
1da177e4	22	#include <net/pkt_sched.h>
cf1facda	23	#include <net/pkt_cls.h>
1da177e4	24
cc7ec456	25	struct prio_sched_data {
1da177e4	26	int bands;
25d8c0d5	27	struct tcf_proto __rcu *filter_list;
6529eaba	28	struct tcf_block *block;
1da177e4 LT	29	u8 prio2band[TC_PRIO_MAX+1];
	30	struct Qdisc *queues[TCQ_PRIO_BANDS];
	31	};
	32
	33
	34	static struct Qdisc *
	35	prio_classify(struct sk_buff skb, struct Qdisc sch, int *qerr)
	36	{
	37	struct prio_sched_data *q = qdisc_priv(sch);
	38	u32 band = skb->priority;
	39	struct tcf_result res;
25d8c0d5	40	struct tcf_proto *fl;
bdba91ec	41	int err;
1da177e4	42
c27f339a	43	*qerr = NET_XMIT_SUCCESS \| __NET_XMIT_BYPASS;
1da177e4	44	if (TC_H_MAJ(skb->priority) != sch->handle) {
25d8c0d5	45	fl = rcu_dereference_bh(q->filter_list);
87d83093	46	err = tcf_classify(skb, fl, &res, false);
1da177e4	47	#ifdef CONFIG_NET_CLS_ACT
dbaaa07a	48	switch (err) {
1da177e4 LT	49	case TC_ACT_STOLEN:
1da177e4 LT	50	case TC_ACT_QUEUED:
e25ea21f	51	case TC_ACT_TRAP:
378a2f09	52	*qerr = NET_XMIT_SUCCESS \| __NET_XMIT_STOLEN;
f3ae608e	53	/* fall through */
1da177e4 LT	54	case TC_ACT_SHOT:
1da177e4 LT	55	return NULL;
3ff50b79	56	}
1da177e4	57	#endif
25d8c0d5	58	if (!fl \|\| err < 0) {
1da177e4 LT	59	if (TC_H_MAJ(band))
1da177e4 LT	60	band = 0;
cc7ec456	61	return q->queues[q->prio2band[band & TC_PRIO_MAX]];
1da177e4 LT	62	}
	63	band = res.classid;
	64	}
	65	band = TC_H_MIN(band) - 1;
3e5c2d3b	66	if (band >= q->bands)
1d8ae3fd DM	67	return q->queues[q->prio2band[0]];
1d8ae3fd DM	68
1da177e4 LT	69	return q->queues[band];
	70	}
	71
	72	static int
520ac30f	73	prio_enqueue(struct sk_buff skb, struct Qdisc sch, struct sk_buff **to_free)
1da177e4 LT	74	{
	75	struct Qdisc *qdisc;
	76	int ret;
	77
	78	qdisc = prio_classify(skb, sch, &ret);
	79	#ifdef CONFIG_NET_CLS_ACT
	80	if (qdisc == NULL) {
29f1df6c	81
c27f339a	82	if (ret & __NET_XMIT_BYPASS)
25331d6c	83	qdisc_qstats_drop(sch);
39ad1297	84	__qdisc_drop(skb, to_free);
1da177e4 LT	85	return ret;
	86	}
	87	#endif
	88
520ac30f	89	ret = qdisc_enqueue(skb, qdisc, to_free);
5f86173b	90	if (ret == NET_XMIT_SUCCESS) {
6529d75a	91	qdisc_qstats_backlog_inc(sch, skb);
1da177e4 LT	92	sch->q.qlen++;
	93	return NET_XMIT_SUCCESS;
	94	}
378a2f09	95	if (net_xmit_drop_count(ret))
25331d6c	96	qdisc_qstats_drop(sch);
10297b99	97	return ret;
1da177e4 LT	98	}
1da177e4 LT	99
48a8f519 PM	100	static struct sk_buff prio_peek(struct Qdisc sch)
	101	{
	102	struct prio_sched_data *q = qdisc_priv(sch);
	103	int prio;
	104
	105	for (prio = 0; prio < q->bands; prio++) {
	106	struct Qdisc *qdisc = q->queues[prio];
	107	struct sk_buff *skb = qdisc->ops->peek(qdisc);
	108	if (skb)
	109	return skb;
	110	}
	111	return NULL;
	112	}
1da177e4	113
cc7ec456	114	static struct sk_buff prio_dequeue(struct Qdisc sch)
1da177e4	115	{
1da177e4 LT	116	struct prio_sched_data *q = qdisc_priv(sch);
1da177e4 LT	117	int prio;
1da177e4 LT	118
1da177e4 LT	119	for (prio = 0; prio < q->bands; prio++) {
1d8ae3fd	120	struct Qdisc *qdisc = q->queues[prio];
3557619f	121	struct sk_buff *skb = qdisc_dequeue_peeked(qdisc);
1d8ae3fd	122	if (skb) {
9190b3b3	123	qdisc_bstats_update(sch, skb);
6529d75a	124	qdisc_qstats_backlog_dec(sch, skb);
1d8ae3fd DM	125	sch->q.qlen--;
1d8ae3fd DM	126	return skb;
1da177e4 LT	127	}
	128	}
	129	return NULL;
	130
	131	}
	132
1da177e4	133	static void
cc7ec456	134	prio_reset(struct Qdisc *sch)
1da177e4 LT	135	{
	136	int prio;
	137	struct prio_sched_data *q = qdisc_priv(sch);
	138
cc7ec456	139	for (prio = 0; prio < q->bands; prio++)
1da177e4	140	qdisc_reset(q->queues[prio]);
6529d75a	141	sch->qstats.backlog = 0;
1da177e4 LT	142	sch->q.qlen = 0;
	143	}
	144
98ceb7b6	145	static int prio_offload(struct Qdisc sch, struct tc_prio_qopt qopt)
7fdb61b4	146	{
7fdb61b4 NF	147	struct net_device *dev = qdisc_dev(sch);
	148	struct tc_prio_qopt_offload opt = {
	149	.handle = sch->handle,
	150	.parent = sch->parent,
	151	};
	152
	153	if (!tc_can_offload(dev) \|\| !dev->netdev_ops->ndo_setup_tc)
	154	return -EOPNOTSUPP;
	155
98ceb7b6	156	if (qopt) {
7fdb61b4	157	opt.command = TC_PRIO_REPLACE;
98ceb7b6 NF	158	opt.replace_params.bands = qopt->bands;
98ceb7b6 NF	159	memcpy(&opt.replace_params.priomap, qopt->priomap,
7fdb61b4 NF	160	TC_PRIO_MAX + 1);
	161	opt.replace_params.qstats = &sch->qstats;
	162	} else {
	163	opt.command = TC_PRIO_DESTROY;
	164	}
	165
	166	return dev->netdev_ops->ndo_setup_tc(dev, TC_SETUP_QDISC_PRIO, &opt);
	167	}
	168
1da177e4	169	static void
cc7ec456	170	prio_destroy(struct Qdisc *sch)
1da177e4 LT	171	{
	172	int prio;
	173	struct prio_sched_data *q = qdisc_priv(sch);
1da177e4	174
6529eaba	175	tcf_block_put(q->block);
98ceb7b6	176	prio_offload(sch, NULL);
cc7ec456	177	for (prio = 0; prio < q->bands; prio++)
1da177e4 LT	178	qdisc_destroy(q->queues[prio]);
	179	}
	180
2030721c AA	181	static int prio_tune(struct Qdisc sch, struct nlattr opt,
2030721c AA	182	struct netlink_ext_ack *extack)
1da177e4 LT	183	{
1da177e4 LT	184	struct prio_sched_data *q = qdisc_priv(sch);
3d7c8257 ED	185	struct Qdisc *queues[TCQ_PRIO_BANDS];
3d7c8257 ED	186	int oldbands = q->bands, i;
d62733c8	187	struct tc_prio_qopt *qopt;
1da177e4	188
1d8ae3fd DM	189	if (nla_len(opt) < sizeof(*qopt))
	190	return -EINVAL;
	191	qopt = nla_data(opt);
d62733c8	192
1d8ae3fd	193	if (qopt->bands > TCQ_PRIO_BANDS \|\| qopt->bands < 2)
1da177e4 LT	194	return -EINVAL;
1da177e4 LT	195
cc7ec456	196	for (i = 0; i <= TC_PRIO_MAX; i++) {
1d8ae3fd	197	if (qopt->priomap[i] >= qopt->bands)
1da177e4 LT	198	return -EINVAL;
	199	}
	200
3d7c8257 ED	201	/* Before commit, make sure we can allocate all new qdiscs */
	202	for (i = oldbands; i < qopt->bands; i++) {
	203	queues[i] = qdisc_create_dflt(sch->dev_queue, &pfifo_qdisc_ops,
a38a9882 AA	204	TC_H_MAKE(sch->handle, i + 1),
a38a9882 AA	205	extack);
3d7c8257 ED	206	if (!queues[i]) {
	207	while (i > oldbands)
	208	qdisc_destroy(queues[--i]);
	209	return -ENOMEM;
	210	}
	211	}
	212
98ceb7b6	213	prio_offload(sch, qopt);
1da177e4	214	sch_tree_lock(sch);
1d8ae3fd	215	q->bands = qopt->bands;
1da177e4 LT	216	memcpy(q->prio2band, qopt->priomap, TC_PRIO_MAX+1);
1da177e4 LT	217
3d7c8257	218	for (i = q->bands; i < oldbands; i++) {
b94c8afc	219	struct Qdisc *child = q->queues[i];
1da177e4	220
3d7c8257 ED	221	qdisc_tree_reduce_backlog(child, child->q.qlen,
	222	child->qstats.backlog);
	223	qdisc_destroy(child);
1da177e4	224	}
cbdf4511	225
49b49971	226	for (i = oldbands; i < q->bands; i++) {
3d7c8257	227	q->queues[i] = queues[i];
49b49971 JK	228	if (q->queues[i] != &noop_qdisc)
	229	qdisc_hash_add(q->queues[i], true);
	230	}
cbdf4511	231
3d7c8257	232	sch_tree_unlock(sch);
1da177e4 LT	233	return 0;
	234	}
	235
e63d7dfd AA	236	static int prio_init(struct Qdisc sch, struct nlattr opt,
e63d7dfd AA	237	struct netlink_ext_ack *extack)
1da177e4	238	{
6529eaba JP	239	struct prio_sched_data *q = qdisc_priv(sch);
	240	int err;
	241
3d7c8257	242	if (!opt)
1da177e4	243	return -EINVAL;
1da177e4	244
8d1a77f9	245	err = tcf_block_get(&q->block, &q->filter_list, sch, extack);
6529eaba JP	246	if (err)
	247	return err;
	248
2030721c	249	return prio_tune(sch, opt, extack);
1da177e4 LT	250	}
1da177e4 LT	251
7fdb61b4 NF	252	static int prio_dump_offload(struct Qdisc *sch)
	253	{
	254	struct net_device *dev = qdisc_dev(sch);
	255	struct tc_prio_qopt_offload hw_stats = {
ef58ca38	256	.command = TC_PRIO_STATS,
7fdb61b4 NF	257	.handle = sch->handle,
7fdb61b4 NF	258	.parent = sch->parent,
ef58ca38 AM	259	{
	260	.stats = {
	261	.bstats = &sch->bstats,
	262	.qstats = &sch->qstats,
	263	},
	264	},
7fdb61b4 NF	265	};
	266	int err;
	267
	268	sch->flags &= ~TCQ_F_OFFLOADED;
	269	if (!tc_can_offload(dev) \|\| !dev->netdev_ops->ndo_setup_tc)
	270	return 0;
	271
	272	err = dev->netdev_ops->ndo_setup_tc(dev, TC_SETUP_QDISC_PRIO,
	273	&hw_stats);
	274	if (err == -EOPNOTSUPP)
	275	return 0;
	276
	277	if (!err)
	278	sch->flags \|= TCQ_F_OFFLOADED;
	279
	280	return err;
	281	}
	282
1da177e4 LT	283	static int prio_dump(struct Qdisc sch, struct sk_buff skb)
	284	{
	285	struct prio_sched_data *q = qdisc_priv(sch);
27a884dc	286	unsigned char *b = skb_tail_pointer(skb);
1da177e4	287	struct tc_prio_qopt opt;
7fdb61b4	288	int err;
1da177e4 LT	289
1da177e4 LT	290	opt.bands = q->bands;
cc7ec456	291	memcpy(&opt.priomap, q->prio2band, TC_PRIO_MAX + 1);
d62733c8	292
7fdb61b4 NF	293	err = prio_dump_offload(sch);
	294	if (err)
	295	goto nla_put_failure;
	296
1b34ec43 DM	297	if (nla_put(skb, TCA_OPTIONS, sizeof(opt), &opt))
1b34ec43 DM	298	goto nla_put_failure;
d62733c8	299
1da177e4 LT	300	return skb->len;
1da177e4 LT	301
1e90474c	302	nla_put_failure:
dc5fc579	303	nlmsg_trim(skb, b);
1da177e4 LT	304	return -1;
	305	}
	306
	307	static int prio_graft(struct Qdisc sch, unsigned long arg, struct Qdisc new,
653d6fd6	308	struct Qdisc *old, struct netlink_ext_ack extack)
1da177e4 LT	309	{
1da177e4 LT	310	struct prio_sched_data *q = qdisc_priv(sch);
b9c7a7ac NF	311	struct tc_prio_qopt_offload graft_offload;
b9c7a7ac NF	312	struct net_device *dev = qdisc_dev(sch);
1da177e4	313	unsigned long band = arg - 1;
b9c7a7ac NF	314	bool any_qdisc_is_offloaded;
b9c7a7ac NF	315	int err;
1da177e4	316
1da177e4 LT	317	if (new == NULL)
	318	new = &noop_qdisc;
	319
86a7996c	320	*old = qdisc_replace(sch, new, &q->queues[band]);
b9c7a7ac NF	321
	322	if (!tc_can_offload(dev))
	323	return 0;
	324
	325	graft_offload.handle = sch->handle;
	326	graft_offload.parent = sch->parent;
	327	graft_offload.graft_params.band = band;
	328	graft_offload.graft_params.child_handle = new->handle;
	329	graft_offload.command = TC_PRIO_GRAFT;
	330
	331	err = dev->netdev_ops->ndo_setup_tc(dev, TC_SETUP_QDISC_PRIO,
	332	&graft_offload);
	333
	334	/* Don't report error if the graft is part of destroy operation. */
	335	if (err && new != &noop_qdisc) {
	336	/* Don't report error if the parent, the old child and the new
	337	* one are not offloaded.
	338	*/
	339	any_qdisc_is_offloaded = sch->flags & TCQ_F_OFFLOADED;
	340	any_qdisc_is_offloaded \|= new->flags & TCQ_F_OFFLOADED;
	341	if (*old)
	342	any_qdisc_is_offloaded \|= (*old)->flags &
	343	TCQ_F_OFFLOADED;
	344
	345	if (any_qdisc_is_offloaded)
	346	NL_SET_ERR_MSG(extack, "Offloading graft operation failed.");
	347	}
	348
1da177e4 LT	349	return 0;
	350	}
	351
	352	static struct Qdisc *
	353	prio_leaf(struct Qdisc *sch, unsigned long arg)
	354	{
	355	struct prio_sched_data *q = qdisc_priv(sch);
	356	unsigned long band = arg - 1;
	357
1da177e4 LT	358	return q->queues[band];
	359	}
	360
143976ce	361	static unsigned long prio_find(struct Qdisc *sch, u32 classid)
1da177e4 LT	362	{
	363	struct prio_sched_data *q = qdisc_priv(sch);
	364	unsigned long band = TC_H_MIN(classid);
	365
	366	if (band - 1 >= q->bands)
	367	return 0;
	368	return band;
	369	}
	370
	371	static unsigned long prio_bind(struct Qdisc *sch, unsigned long parent, u32 classid)
	372	{
143976ce	373	return prio_find(sch, classid);
1da177e4 LT	374	}
	375
	376
143976ce	377	static void prio_unbind(struct Qdisc *q, unsigned long cl)
1da177e4	378	{
1da177e4 LT	379	}
1da177e4 LT	380
1da177e4 LT	381	static int prio_dump_class(struct Qdisc sch, unsigned long cl, struct sk_buff skb,
	382	struct tcmsg *tcm)
	383	{
	384	struct prio_sched_data *q = qdisc_priv(sch);
	385
1da177e4	386	tcm->tcm_handle \|= TC_H_MIN(cl);
5b9a9ccf	387	tcm->tcm_info = q->queues[cl-1]->handle;
1da177e4 LT	388	return 0;
	389	}
	390
2cf6c36c JP	391	static int prio_dump_class_stats(struct Qdisc *sch, unsigned long cl,
	392	struct gnet_dump *d)
	393	{
	394	struct prio_sched_data *q = qdisc_priv(sch);
	395	struct Qdisc *cl_q;
	396
	397	cl_q = q->queues[cl - 1];
edb09eb1 ED	398	if (gnet_stats_copy_basic(qdisc_root_sleeping_running(sch),
edb09eb1 ED	399	d, NULL, &cl_q->bstats) < 0 \|\|
b0ab6f92	400	gnet_stats_copy_queue(d, NULL, &cl_q->qstats, cl_q->q.qlen) < 0)
2cf6c36c JP	401	return -1;
	402
	403	return 0;
	404	}
	405
1da177e4 LT	406	static void prio_walk(struct Qdisc sch, struct qdisc_walker arg)
	407	{
	408	struct prio_sched_data *q = qdisc_priv(sch);
	409	int prio;
	410
	411	if (arg->stop)
	412	return;
	413
	414	for (prio = 0; prio < q->bands; prio++) {
	415	if (arg->count < arg->skip) {
	416	arg->count++;
	417	continue;
	418	}
cc7ec456	419	if (arg->fn(sch, prio + 1, arg) < 0) {
1da177e4 LT	420	arg->stop = 1;
	421	break;
	422	}
	423	arg->count++;
	424	}
	425	}
	426
cbaacc4e AA	427	static struct tcf_block prio_tcf_block(struct Qdisc sch, unsigned long cl,
cbaacc4e AA	428	struct netlink_ext_ack *extack)
1da177e4 LT	429	{
	430	struct prio_sched_data *q = qdisc_priv(sch);
	431
	432	if (cl)
	433	return NULL;
6529eaba	434	return q->block;
1da177e4 LT	435	}
1da177e4 LT	436
20fea08b	437	static const struct Qdisc_class_ops prio_class_ops = {
1da177e4 LT	438	.graft = prio_graft,
1da177e4 LT	439	.leaf = prio_leaf,
143976ce	440	.find = prio_find,
1da177e4	441	.walk = prio_walk,
6529eaba	442	.tcf_block = prio_tcf_block,
1da177e4	443	.bind_tcf = prio_bind,
143976ce	444	.unbind_tcf = prio_unbind,
1da177e4	445	.dump = prio_dump_class,
2cf6c36c	446	.dump_stats = prio_dump_class_stats,
1da177e4 LT	447	};
1da177e4 LT	448
20fea08b	449	static struct Qdisc_ops prio_qdisc_ops __read_mostly = {
1da177e4 LT	450	.next = NULL,
	451	.cl_ops = &prio_class_ops,
	452	.id = "prio",
	453	.priv_size = sizeof(struct prio_sched_data),
	454	.enqueue = prio_enqueue,
	455	.dequeue = prio_dequeue,
48a8f519	456	.peek = prio_peek,
1da177e4 LT	457	.init = prio_init,
	458	.reset = prio_reset,
	459	.destroy = prio_destroy,
	460	.change = prio_tune,
	461	.dump = prio_dump,
	462	.owner = THIS_MODULE,
	463	};
	464
	465	static int __init prio_module_init(void)
	466	{
1d8ae3fd	467	return register_qdisc(&prio_qdisc_ops);
1da177e4 LT	468	}
1da177e4 LT	469
10297b99	470	static void __exit prio_module_exit(void)
1da177e4 LT	471	{
	472	unregister_qdisc(&prio_qdisc_ops);
	473	}
	474
	475	module_init(prio_module_init)
	476	module_exit(prio_module_exit)
	477
	478	MODULE_LICENSE("GPL");