[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, bool enable)
{
	struct prio_sched_data *q = qdisc_priv(sch);
	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 (enable) {
		opt.command = TC_PRIO_REPLACE;
		opt.replace_params.bands = q->bands;
		memcpy(&opt.replace_params.priomap, q->prio2band,
		       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, false);
	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;
		}
	}

	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);
	prio_offload(sch, true);
	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);
	unsigned long band = arg - 1;

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

	*old = qdisc_replace(sch, new, &q->queues[band]);
	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
7fdb61b4 NF	145	static int prio_offload(struct Qdisc *sch, bool enable)
	146	{
	147	struct prio_sched_data *q = qdisc_priv(sch);
	148	struct net_device *dev = qdisc_dev(sch);
	149	struct tc_prio_qopt_offload opt = {
	150	.handle = sch->handle,
	151	.parent = sch->parent,
	152	};
	153
	154	if (!tc_can_offload(dev) \|\| !dev->netdev_ops->ndo_setup_tc)
	155	return -EOPNOTSUPP;
	156
	157	if (enable) {
	158	opt.command = TC_PRIO_REPLACE;
	159	opt.replace_params.bands = q->bands;
	160	memcpy(&opt.replace_params.priomap, q->prio2band,
	161	TC_PRIO_MAX + 1);
	162	opt.replace_params.qstats = &sch->qstats;
	163	} else {
	164	opt.command = TC_PRIO_DESTROY;
	165	}
	166
	167	return dev->netdev_ops->ndo_setup_tc(dev, TC_SETUP_QDISC_PRIO, &opt);
	168	}
	169
1da177e4	170	static void
cc7ec456	171	prio_destroy(struct Qdisc *sch)
1da177e4 LT	172	{
	173	int prio;
	174	struct prio_sched_data *q = qdisc_priv(sch);
1da177e4	175
6529eaba	176	tcf_block_put(q->block);
7fdb61b4	177	prio_offload(sch, false);
cc7ec456	178	for (prio = 0; prio < q->bands; prio++)
1da177e4 LT	179	qdisc_destroy(q->queues[prio]);
	180	}
	181
2030721c AA	182	static int prio_tune(struct Qdisc sch, struct nlattr opt,
2030721c AA	183	struct netlink_ext_ack *extack)
1da177e4 LT	184	{
1da177e4 LT	185	struct prio_sched_data *q = qdisc_priv(sch);
3d7c8257 ED	186	struct Qdisc *queues[TCQ_PRIO_BANDS];
3d7c8257 ED	187	int oldbands = q->bands, i;
d62733c8	188	struct tc_prio_qopt *qopt;
1da177e4	189
1d8ae3fd DM	190	if (nla_len(opt) < sizeof(*qopt))
	191	return -EINVAL;
	192	qopt = nla_data(opt);
d62733c8	193
1d8ae3fd	194	if (qopt->bands > TCQ_PRIO_BANDS \|\| qopt->bands < 2)
1da177e4 LT	195	return -EINVAL;
1da177e4 LT	196
cc7ec456	197	for (i = 0; i <= TC_PRIO_MAX; i++) {
1d8ae3fd	198	if (qopt->priomap[i] >= qopt->bands)
1da177e4 LT	199	return -EINVAL;
	200	}
	201
3d7c8257 ED	202	/* Before commit, make sure we can allocate all new qdiscs */
	203	for (i = oldbands; i < qopt->bands; i++) {
	204	queues[i] = qdisc_create_dflt(sch->dev_queue, &pfifo_qdisc_ops,
a38a9882 AA	205	TC_H_MAKE(sch->handle, i + 1),
a38a9882 AA	206	extack);
3d7c8257 ED	207	if (!queues[i]) {
	208	while (i > oldbands)
	209	qdisc_destroy(queues[--i]);
	210	return -ENOMEM;
	211	}
	212	}
	213
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);
7fdb61b4	233	prio_offload(sch, true);
1da177e4 LT	234	return 0;
	235	}
	236
e63d7dfd AA	237	static int prio_init(struct Qdisc sch, struct nlattr opt,
e63d7dfd AA	238	struct netlink_ext_ack *extack)
1da177e4	239	{
6529eaba JP	240	struct prio_sched_data *q = qdisc_priv(sch);
	241	int err;
	242
3d7c8257	243	if (!opt)
1da177e4	244	return -EINVAL;
1da177e4	245
8d1a77f9	246	err = tcf_block_get(&q->block, &q->filter_list, sch, extack);
6529eaba JP	247	if (err)
	248	return err;
	249
2030721c	250	return prio_tune(sch, opt, extack);
1da177e4 LT	251	}
1da177e4 LT	252
7fdb61b4 NF	253	static int prio_dump_offload(struct Qdisc *sch)
	254	{
	255	struct net_device *dev = qdisc_dev(sch);
	256	struct tc_prio_qopt_offload hw_stats = {
ef58ca38	257	.command = TC_PRIO_STATS,
7fdb61b4 NF	258	.handle = sch->handle,
7fdb61b4 NF	259	.parent = sch->parent,
ef58ca38 AM	260	{
	261	.stats = {
	262	.bstats = &sch->bstats,
	263	.qstats = &sch->qstats,
	264	},
	265	},
7fdb61b4 NF	266	};
	267	int err;
	268
	269	sch->flags &= ~TCQ_F_OFFLOADED;
	270	if (!tc_can_offload(dev) \|\| !dev->netdev_ops->ndo_setup_tc)
	271	return 0;
	272
	273	err = dev->netdev_ops->ndo_setup_tc(dev, TC_SETUP_QDISC_PRIO,
	274	&hw_stats);
	275	if (err == -EOPNOTSUPP)
	276	return 0;
	277
	278	if (!err)
	279	sch->flags \|= TCQ_F_OFFLOADED;
	280
	281	return err;
	282	}
	283
1da177e4 LT	284	static int prio_dump(struct Qdisc sch, struct sk_buff skb)
	285	{
	286	struct prio_sched_data *q = qdisc_priv(sch);
27a884dc	287	unsigned char *b = skb_tail_pointer(skb);
1da177e4	288	struct tc_prio_qopt opt;
7fdb61b4	289	int err;
1da177e4 LT	290
1da177e4 LT	291	opt.bands = q->bands;
cc7ec456	292	memcpy(&opt.priomap, q->prio2band, TC_PRIO_MAX + 1);
d62733c8	293
7fdb61b4 NF	294	err = prio_dump_offload(sch);
	295	if (err)
	296	goto nla_put_failure;
	297
1b34ec43 DM	298	if (nla_put(skb, TCA_OPTIONS, sizeof(opt), &opt))
1b34ec43 DM	299	goto nla_put_failure;
d62733c8	300
1da177e4 LT	301	return skb->len;
1da177e4 LT	302
1e90474c	303	nla_put_failure:
dc5fc579	304	nlmsg_trim(skb, b);
1da177e4 LT	305	return -1;
	306	}
	307
	308	static int prio_graft(struct Qdisc sch, unsigned long arg, struct Qdisc new,
653d6fd6	309	struct Qdisc *old, struct netlink_ext_ack extack)
1da177e4 LT	310	{
	311	struct prio_sched_data *q = qdisc_priv(sch);
	312	unsigned long band = arg - 1;
	313
1da177e4 LT	314	if (new == NULL)
	315	new = &noop_qdisc;
	316
86a7996c	317	*old = qdisc_replace(sch, new, &q->queues[band]);
1da177e4 LT	318	return 0;
	319	}
	320
	321	static struct Qdisc *
	322	prio_leaf(struct Qdisc *sch, unsigned long arg)
	323	{
	324	struct prio_sched_data *q = qdisc_priv(sch);
	325	unsigned long band = arg - 1;
	326
1da177e4 LT	327	return q->queues[band];
	328	}
	329
143976ce	330	static unsigned long prio_find(struct Qdisc *sch, u32 classid)
1da177e4 LT	331	{
	332	struct prio_sched_data *q = qdisc_priv(sch);
	333	unsigned long band = TC_H_MIN(classid);
	334
	335	if (band - 1 >= q->bands)
	336	return 0;
	337	return band;
	338	}
	339
	340	static unsigned long prio_bind(struct Qdisc *sch, unsigned long parent, u32 classid)
	341	{
143976ce	342	return prio_find(sch, classid);
1da177e4 LT	343	}
	344
	345
143976ce	346	static void prio_unbind(struct Qdisc *q, unsigned long cl)
1da177e4	347	{
1da177e4 LT	348	}
1da177e4 LT	349
1da177e4 LT	350	static int prio_dump_class(struct Qdisc sch, unsigned long cl, struct sk_buff skb,
	351	struct tcmsg *tcm)
	352	{
	353	struct prio_sched_data *q = qdisc_priv(sch);
	354
1da177e4	355	tcm->tcm_handle \|= TC_H_MIN(cl);
5b9a9ccf	356	tcm->tcm_info = q->queues[cl-1]->handle;
1da177e4 LT	357	return 0;
	358	}
	359
2cf6c36c JP	360	static int prio_dump_class_stats(struct Qdisc *sch, unsigned long cl,
	361	struct gnet_dump *d)
	362	{
	363	struct prio_sched_data *q = qdisc_priv(sch);
	364	struct Qdisc *cl_q;
	365
	366	cl_q = q->queues[cl - 1];
edb09eb1 ED	367	if (gnet_stats_copy_basic(qdisc_root_sleeping_running(sch),
edb09eb1 ED	368	d, NULL, &cl_q->bstats) < 0 \|\|
b0ab6f92	369	gnet_stats_copy_queue(d, NULL, &cl_q->qstats, cl_q->q.qlen) < 0)
2cf6c36c JP	370	return -1;
	371
	372	return 0;
	373	}
	374
1da177e4 LT	375	static void prio_walk(struct Qdisc sch, struct qdisc_walker arg)
	376	{
	377	struct prio_sched_data *q = qdisc_priv(sch);
	378	int prio;
	379
	380	if (arg->stop)
	381	return;
	382
	383	for (prio = 0; prio < q->bands; prio++) {
	384	if (arg->count < arg->skip) {
	385	arg->count++;
	386	continue;
	387	}
cc7ec456	388	if (arg->fn(sch, prio + 1, arg) < 0) {
1da177e4 LT	389	arg->stop = 1;
	390	break;
	391	}
	392	arg->count++;
	393	}
	394	}
	395
cbaacc4e AA	396	static struct tcf_block prio_tcf_block(struct Qdisc sch, unsigned long cl,
cbaacc4e AA	397	struct netlink_ext_ack *extack)
1da177e4 LT	398	{
	399	struct prio_sched_data *q = qdisc_priv(sch);
	400
	401	if (cl)
	402	return NULL;
6529eaba	403	return q->block;
1da177e4 LT	404	}
1da177e4 LT	405
20fea08b	406	static const struct Qdisc_class_ops prio_class_ops = {
1da177e4 LT	407	.graft = prio_graft,
1da177e4 LT	408	.leaf = prio_leaf,
143976ce	409	.find = prio_find,
1da177e4	410	.walk = prio_walk,
6529eaba	411	.tcf_block = prio_tcf_block,
1da177e4	412	.bind_tcf = prio_bind,
143976ce	413	.unbind_tcf = prio_unbind,
1da177e4	414	.dump = prio_dump_class,
2cf6c36c	415	.dump_stats = prio_dump_class_stats,
1da177e4 LT	416	};
1da177e4 LT	417
20fea08b	418	static struct Qdisc_ops prio_qdisc_ops __read_mostly = {
1da177e4 LT	419	.next = NULL,
	420	.cl_ops = &prio_class_ops,
	421	.id = "prio",
	422	.priv_size = sizeof(struct prio_sched_data),
	423	.enqueue = prio_enqueue,
	424	.dequeue = prio_dequeue,
48a8f519	425	.peek = prio_peek,
1da177e4 LT	426	.init = prio_init,
	427	.reset = prio_reset,
	428	.destroy = prio_destroy,
	429	.change = prio_tune,
	430	.dump = prio_dump,
	431	.owner = THIS_MODULE,
	432	};
	433
	434	static int __init prio_module_init(void)
	435	{
1d8ae3fd	436	return register_qdisc(&prio_qdisc_ops);
1da177e4 LT	437	}
1da177e4 LT	438
10297b99	439	static void __exit prio_module_exit(void)
1da177e4 LT	440	{
	441	unregister_qdisc(&prio_qdisc_ops);
	442	}
	443
	444	module_init(prio_module_init)
	445	module_exit(prio_module_exit)
	446
	447	MODULE_LICENSE("GPL");