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

/*
 * net/sched/sch_mqprio.c
 *
 * Copyright (c) 2010 John Fastabend <john.r.fastabend@intel.com>
 *
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License
 * version 2 as published by the Free Software Foundation.
 */

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

struct mqprio_sched {
	struct Qdisc		**qdiscs;
	u16 mode;
	u16 shaper;
	int hw_offload;
	u32 flags;
	u64 min_rate[TC_QOPT_MAX_QUEUE];
	u64 max_rate[TC_QOPT_MAX_QUEUE];
};

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

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

	if (priv->hw_offload && dev->netdev_ops->ndo_setup_tc) {
		struct tc_mqprio_qopt_offload mqprio = { { 0 } };

		switch (priv->mode) {
		case TC_MQPRIO_MODE_DCB:
		case TC_MQPRIO_MODE_CHANNEL:
			dev->netdev_ops->ndo_setup_tc(dev,
						      TC_SETUP_QDISC_MQPRIO,
						      &mqprio);
			break;
		default:
			return;
		}
	} else {
		netdev_set_num_tc(dev, 0);
	}
}

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;
	}

	/* 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;

	/* 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.
	 */
	if (qopt->hw)
		return dev->netdev_ops->ndo_setup_tc ? 0 : -EINVAL;

	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;
}

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))
		return nla_parse(tb, maxtype, nla_data(nla) + NLA_ALIGN(len),
				 nested_len, policy, NULL);

	memset(tb, 0, sizeof(struct nlattr *) * (maxtype + 1));
	return 0;
}

static int mqprio_init(struct Qdisc *sch, struct nlattr *opt,
		       struct netlink_ext_ack *extack)
{
	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;
	struct nlattr *tb[TCA_MQPRIO_MAX + 1];
	struct nlattr *attr;
	int rem;
	int len;

	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;

	/* make certain can allocate enough classids to handle queues */
	if (dev->num_tx_queues >= TC_H_MIN_PRIORITY)
		return -ENOMEM;

	if (!opt || nla_len(opt) < sizeof(*qopt))
		return -EINVAL;

	qopt = nla_data(opt);
	if (mqprio_parse_opt(dev, qopt))
		return -EINVAL;

	len = nla_len(opt) - NLA_ALIGN(sizeof(*qopt));
	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;
		}
	}

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

	for (i = 0; i < dev->num_tx_queues; i++) {
		dev_queue = netdev_get_tx_queue(dev, i);
		qdisc = qdisc_create_dflt(dev_queue,
					  get_default_qdisc_ops(dev, i),
					  TC_H_MAKE(TC_H_MAJ(sch->handle),
						    TC_H_MIN(i + 1)), extack);
		if (!qdisc)
			return -ENOMEM;

		priv->qdiscs[i] = qdisc;
		qdisc->flags |= TCQ_F_ONETXQUEUE | TCQ_F_NOPARENT;
	}

	/* 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) {
		struct tc_mqprio_qopt_offload mqprio = {.qopt = *qopt};

		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,
						    TC_SETUP_QDISC_MQPRIO,
						    &mqprio);
		if (err)
			return err;

		priv->hw_offload = mqprio.qopt.hw;
	} 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;
}

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

	/* Attach underlying qdisc */
	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);
		if (ntx < dev->real_num_tx_queues)
			qdisc_hash_add(qdisc, false);
	}
	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);
	unsigned long ntx = cl - 1;

	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,
			struct Qdisc **old, struct netlink_ext_ack *extack)
{
	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);

	if (new)
		new->flags |= TCQ_F_ONETXQUEUE | TCQ_F_NOPARENT;

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

	return 0;
}

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) {
		nest = nla_nest_start(skb, TCA_MQPRIO_MIN_RATE64);
		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) {
		nest = nla_nest_start(skb, TCA_MQPRIO_MAX_RATE64);
		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;
}

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);
	struct nlattr *nla = (struct nlattr *)skb_tail_pointer(skb);
	struct tc_mqprio_qopt opt = { 0 };
	struct Qdisc *qdisc;
	unsigned int ntx, tc;

	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)) {
			__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);
		} 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;
		}

		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));
	opt.hw = priv->hw_offload;

	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;
	}

	if (nla_put(skb, TCA_OPTIONS, NLA_ALIGN(sizeof(opt)), &opt))
		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))
		goto nla_put_failure;

	return nla_nest_end(skb, nla);
nla_put_failure:
	nlmsg_trim(skb, nla);
	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;
}

static unsigned long mqprio_find(struct Qdisc *sch, u32 classid)
{
	struct net_device *dev = qdisc_dev(sch);
	unsigned int ntx = TC_H_MIN(classid);

	/* 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;
}

static int mqprio_dump_class(struct Qdisc *sch, unsigned long cl,
			 struct sk_buff *skb, struct tcmsg *tcm)
{
	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);

		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 {
		tcm->tcm_parent = TC_H_ROOT;
		tcm->tcm_info = 0;
	}
	tcm->tcm_handle |= TC_H_MIN(cl);
	return 0;
}

static int mqprio_dump_class_stats(struct Qdisc *sch, unsigned long cl,
				   struct gnet_dump *d)
	__releases(d->lock)
	__acquires(d->lock)
{
	if (cl >= TC_H_MIN_PRIORITY) {
		int i;
		__u32 qlen = 0;
		struct gnet_stats_queue qstats = {0};
		struct gnet_stats_basic_packed bstats = {0};
		struct net_device *dev = qdisc_dev(sch);
		struct netdev_tc_txq tc = dev->tc_to_txq[cl & TC_BITMASK];

		/* 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.
		 */
		if (d->lock)
			spin_unlock_bh(d->lock);

		for (i = tc.offset; i < tc.offset + tc.count; i++) {
			struct netdev_queue *q = netdev_get_tx_queue(dev, i);
			struct Qdisc *qdisc = rtnl_dereference(q->qdisc);
			struct gnet_stats_basic_cpu __percpu *cpu_bstats = NULL;
			struct gnet_stats_queue __percpu *cpu_qstats = NULL;

			spin_lock_bh(qdisc_lock(qdisc));
			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);
			spin_unlock_bh(qdisc_lock(qdisc));
		}

		/* Reclaim root sleeping lock before completing stats */
		if (d->lock)
			spin_lock_bh(d->lock);
		if (gnet_stats_copy_basic(NULL, d, NULL, &bstats) < 0 ||
		    gnet_stats_copy_queue(d, NULL, &qstats, qlen) < 0)
			return -1;
	} else {
		struct netdev_queue *dev_queue = mqprio_queue_get(sch, cl);

		sch = dev_queue->qdisc_sleeping;
		if (gnet_stats_copy_basic(qdisc_root_sleeping_running(sch),
					  d, NULL, &sch->bstats) < 0 ||
		    gnet_stats_copy_queue(d, NULL,
					  &sch->qstats, sch->q.qlen) < 0)
			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;
	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++) {
		if (arg->fn(sch, ntx + 1, arg) < 0) {
			arg->stop = 1;
			return;
		}
		arg->count++;
	}
}

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

static const struct Qdisc_class_ops mqprio_class_ops = {
	.graft		= mqprio_graft,
	.leaf		= mqprio_leaf,
	.find		= mqprio_find,
	.walk		= mqprio_walk,
	.dump		= mqprio_dump_class,
	.dump_stats	= mqprio_dump_class_stats,
	.select_queue	= mqprio_select_queue,
};

static struct Qdisc_ops mqprio_qdisc_ops __read_mostly = {
	.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");
Commit	Line	Data
b8970f0b JF	1	/*
	2	* net/sched/sch_mqprio.c
	3	*
	4	* Copyright (c) 2010 John Fastabend <john.r.fastabend@intel.com>
	5	*
	6	* This program is free software; you can redistribute it and/or
	7	* modify it under the terms of the GNU General Public License
	8	* version 2 as published by the Free Software Foundation.
	9	*/
	10
	11	#include <linux/types.h>
	12	#include <linux/slab.h>
	13	#include <linux/kernel.h>
	14	#include <linux/string.h>
	15	#include <linux/errno.h>
	16	#include <linux/skbuff.h>
3a9a231d	17	#include <linux/module.h>
b8970f0b JF	18	#include <net/netlink.h>
	19	#include <net/pkt_sched.h>
	20	#include <net/sch_generic.h>
4e8b86c0	21	#include <net/pkt_cls.h>
b8970f0b JF	22
	23	struct mqprio_sched {
	24	struct Qdisc **qdiscs;
4e8b86c0 AN	25	u16 mode;
4e8b86c0 AN	26	u16 shaper;
2026fecf	27	int hw_offload;
4e8b86c0 AN	28	u32 flags;
	29	u64 min_rate[TC_QOPT_MAX_QUEUE];
	30	u64 max_rate[TC_QOPT_MAX_QUEUE];
b8970f0b JF	31	};
	32
	33	static void mqprio_destroy(struct Qdisc *sch)
	34	{
	35	struct net_device *dev = qdisc_dev(sch);
	36	struct mqprio_sched *priv = qdisc_priv(sch);
	37	unsigned int ntx;
	38
ac7100ba BH	39	if (priv->qdiscs) {
	40	for (ntx = 0;
	41	ntx < dev->num_tx_queues && priv->qdiscs[ntx];
	42	ntx++)
86bd446b	43	qdisc_put(priv->qdiscs[ntx]);
ac7100ba BH	44	kfree(priv->qdiscs);
ac7100ba BH	45	}
b8970f0b	46
56f36acd	47	if (priv->hw_offload && dev->netdev_ops->ndo_setup_tc) {
4e8b86c0 AN	48	struct tc_mqprio_qopt_offload mqprio = { { 0 } };
	49
	50	switch (priv->mode) {
	51	case TC_MQPRIO_MODE_DCB:
	52	case TC_MQPRIO_MODE_CHANNEL:
575ed7d3 NF	53	dev->netdev_ops->ndo_setup_tc(dev,
575ed7d3 NF	54	TC_SETUP_QDISC_MQPRIO,
4e8b86c0 AN	55	&mqprio);
	56	break;
	57	default:
	58	return;
	59	}
56f36acd	60	} else {
b8970f0b	61	netdev_set_num_tc(dev, 0);
56f36acd	62	}
b8970f0b JF	63	}
	64
	65	static int mqprio_parse_opt(struct net_device dev, struct tc_mqprio_qopt qopt)
	66	{
	67	int i, j;
	68
	69	/* Verify num_tc is not out of max range */
	70	if (qopt->num_tc > TC_MAX_QUEUE)
	71	return -EINVAL;
	72
	73	/* Verify priority mapping uses valid tcs */
	74	for (i = 0; i < TC_BITMASK + 1; i++) {
	75	if (qopt->prio_tc_map[i] >= qopt->num_tc)
	76	return -EINVAL;
	77	}
	78
2026fecf AD	79	/* Limit qopt->hw to maximum supported offload value. Drivers have
	80	* the option of overriding this later if they don't support the a
	81	* given offload type.
	82	*/
	83	if (qopt->hw > TC_MQPRIO_HW_OFFLOAD_MAX)
	84	qopt->hw = TC_MQPRIO_HW_OFFLOAD_MAX;
b8970f0b	85
2026fecf AD	86	/* If hardware offload is requested we will leave it to the device
	87	* to either populate the queue counts itself or to validate the
	88	* provided queue counts. If ndo_setup_tc is not present then
	89	* hardware doesn't support offload and we should return an error.
b8970f0b JF	90	*/
b8970f0b JF	91	if (qopt->hw)
2026fecf	92	return dev->netdev_ops->ndo_setup_tc ? 0 : -EINVAL;
b8970f0b JF	93
	94	for (i = 0; i < qopt->num_tc; i++) {
	95	unsigned int last = qopt->offset[i] + qopt->count[i];
	96
	97	/* Verify the queue count is in tx range being equal to the
	98	* real_num_tx_queues indicates the last queue is in use.
	99	*/
	100	if (qopt->offset[i] >= dev->real_num_tx_queues \|\|
	101	!qopt->count[i] \|\|
	102	last > dev->real_num_tx_queues)
	103	return -EINVAL;
	104
	105	/* Verify that the offset and counts do not overlap */
	106	for (j = i + 1; j < qopt->num_tc; j++) {
	107	if (last > qopt->offset[j])
	108	return -EINVAL;
	109	}
	110	}
	111
	112	return 0;
	113	}
	114
4e8b86c0 AN	115	static const struct nla_policy mqprio_policy[TCA_MQPRIO_MAX + 1] = {
	116	[TCA_MQPRIO_MODE] = { .len = sizeof(u16) },
	117	[TCA_MQPRIO_SHAPER] = { .len = sizeof(u16) },
	118	[TCA_MQPRIO_MIN_RATE64] = { .type = NLA_NESTED },
	119	[TCA_MQPRIO_MAX_RATE64] = { .type = NLA_NESTED },
	120	};
	121
	122	static int parse_attr(struct nlattr tb[], int maxtype, struct nlattr nla,
	123	const struct nla_policy *policy, int len)
	124	{
	125	int nested_len = nla_len(nla) - NLA_ALIGN(len);
	126
	127	if (nested_len >= nla_attr_size(0))
	128	return nla_parse(tb, maxtype, nla_data(nla) + NLA_ALIGN(len),
	129	nested_len, policy, NULL);
	130
	131	memset(tb, 0, sizeof(struct nlattr ) (maxtype + 1));
	132	return 0;
	133	}
	134
e63d7dfd AA	135	static int mqprio_init(struct Qdisc sch, struct nlattr opt,
e63d7dfd AA	136	struct netlink_ext_ack *extack)
b8970f0b JF	137	{
	138	struct net_device *dev = qdisc_dev(sch);
	139	struct mqprio_sched *priv = qdisc_priv(sch);
	140	struct netdev_queue *dev_queue;
	141	struct Qdisc *qdisc;
	142	int i, err = -EOPNOTSUPP;
	143	struct tc_mqprio_qopt *qopt = NULL;
4e8b86c0 AN	144	struct nlattr *tb[TCA_MQPRIO_MAX + 1];
	145	struct nlattr *attr;
	146	int rem;
22ce97fe	147	int len;
b8970f0b JF	148
	149	BUILD_BUG_ON(TC_MAX_QUEUE != TC_QOPT_MAX_QUEUE);
	150	BUILD_BUG_ON(TC_BITMASK != TC_QOPT_BITMASK);
	151
	152	if (sch->parent != TC_H_ROOT)
	153	return -EOPNOTSUPP;
	154
	155	if (!netif_is_multiqueue(dev))
	156	return -EOPNOTSUPP;
	157
32302902 AD	158	/* make certain can allocate enough classids to handle queues */
	159	if (dev->num_tx_queues >= TC_H_MIN_PRIORITY)
	160	return -ENOMEM;
	161
7838f2ce	162	if (!opt \|\| nla_len(opt) < sizeof(*qopt))
b8970f0b JF	163	return -EINVAL;
	164
	165	qopt = nla_data(opt);
	166	if (mqprio_parse_opt(dev, qopt))
	167	return -EINVAL;
	168
22ce97fe	169	len = nla_len(opt) - NLA_ALIGN(sizeof(*qopt));
4e8b86c0 AN	170	if (len > 0) {
	171	err = parse_attr(tb, TCA_MQPRIO_MAX, opt, mqprio_policy,
	172	sizeof(*qopt));
	173	if (err < 0)
	174	return err;
	175
	176	if (!qopt->hw)
	177	return -EINVAL;
	178
	179	if (tb[TCA_MQPRIO_MODE]) {
	180	priv->flags \|= TC_MQPRIO_F_MODE;
	181	priv->mode = (u16 )nla_data(tb[TCA_MQPRIO_MODE]);
	182	}
	183
	184	if (tb[TCA_MQPRIO_SHAPER]) {
	185	priv->flags \|= TC_MQPRIO_F_SHAPER;
	186	priv->shaper = (u16 )nla_data(tb[TCA_MQPRIO_SHAPER]);
	187	}
	188
	189	if (tb[TCA_MQPRIO_MIN_RATE64]) {
	190	if (priv->shaper != TC_MQPRIO_SHAPER_BW_RATE)
	191	return -EINVAL;
	192	i = 0;
	193	nla_for_each_nested(attr, tb[TCA_MQPRIO_MIN_RATE64],
	194	rem) {
	195	if (nla_type(attr) != TCA_MQPRIO_MIN_RATE64)
	196	return -EINVAL;
	197	if (i >= qopt->num_tc)
	198	break;
	199	priv->min_rate[i] = (u64 )nla_data(attr);
	200	i++;
	201	}
	202	priv->flags \|= TC_MQPRIO_F_MIN_RATE;
	203	}
	204
	205	if (tb[TCA_MQPRIO_MAX_RATE64]) {
	206	if (priv->shaper != TC_MQPRIO_SHAPER_BW_RATE)
	207	return -EINVAL;
	208	i = 0;
	209	nla_for_each_nested(attr, tb[TCA_MQPRIO_MAX_RATE64],
	210	rem) {
	211	if (nla_type(attr) != TCA_MQPRIO_MAX_RATE64)
	212	return -EINVAL;
	213	if (i >= qopt->num_tc)
	214	break;
	215	priv->max_rate[i] = (u64 )nla_data(attr);
	216	i++;
	217	}
	218	priv->flags \|= TC_MQPRIO_F_MAX_RATE;
	219	}
	220	}
	221
b8970f0b JF	222	/* pre-allocate qdisc, attachment can't fail */
	223	priv->qdiscs = kcalloc(dev->num_tx_queues, sizeof(priv->qdiscs[0]),
	224	GFP_KERNEL);
87b60cfa ED	225	if (!priv->qdiscs)
87b60cfa ED	226	return -ENOMEM;
b8970f0b JF	227
	228	for (i = 0; i < dev->num_tx_queues; i++) {
	229	dev_queue = netdev_get_tx_queue(dev, i);
1f27cde3 ED	230	qdisc = qdisc_create_dflt(dev_queue,
1f27cde3 ED	231	get_default_qdisc_ops(dev, i),
b8970f0b	232	TC_H_MAKE(TC_H_MAJ(sch->handle),
a38a9882	233	TC_H_MIN(i + 1)), extack);
87b60cfa ED	234	if (!qdisc)
	235	return -ENOMEM;
	236
b8970f0b	237	priv->qdiscs[i] = qdisc;
4eaf3b84	238	qdisc->flags \|= TCQ_F_ONETXQUEUE \| TCQ_F_NOPARENT;
b8970f0b JF	239	}
	240
	241	/* If the mqprio options indicate that hardware should own
	242	* the queue mapping then run ndo_setup_tc otherwise use the
	243	* supplied and verified mapping
	244	*/
	245	if (qopt->hw) {
4e8b86c0	246	struct tc_mqprio_qopt_offload mqprio = {.qopt = *qopt};
16e5cc64	247
4e8b86c0 AN	248	switch (priv->mode) {
	249	case TC_MQPRIO_MODE_DCB:
	250	if (priv->shaper != TC_MQPRIO_SHAPER_DCB)
	251	return -EINVAL;
	252	break;
	253	case TC_MQPRIO_MODE_CHANNEL:
	254	mqprio.flags = priv->flags;
	255	if (priv->flags & TC_MQPRIO_F_MODE)
	256	mqprio.mode = priv->mode;
	257	if (priv->flags & TC_MQPRIO_F_SHAPER)
	258	mqprio.shaper = priv->shaper;
	259	if (priv->flags & TC_MQPRIO_F_MIN_RATE)
	260	for (i = 0; i < mqprio.qopt.num_tc; i++)
	261	mqprio.min_rate[i] = priv->min_rate[i];
	262	if (priv->flags & TC_MQPRIO_F_MAX_RATE)
	263	for (i = 0; i < mqprio.qopt.num_tc; i++)
	264	mqprio.max_rate[i] = priv->max_rate[i];
	265	break;
	266	default:
	267	return -EINVAL;
	268	}
	269	err = dev->netdev_ops->ndo_setup_tc(dev,
575ed7d3	270	TC_SETUP_QDISC_MQPRIO,
de4784ca	271	&mqprio);
b8970f0b	272	if (err)
87b60cfa	273	return err;
2026fecf	274
4e8b86c0	275	priv->hw_offload = mqprio.qopt.hw;
b8970f0b JF	276	} else {
	277	netdev_set_num_tc(dev, qopt->num_tc);
	278	for (i = 0; i < qopt->num_tc; i++)
	279	netdev_set_tc_queue(dev, i,
	280	qopt->count[i], qopt->offset[i]);
	281	}
	282
	283	/* Always use supplied priority mappings */
	284	for (i = 0; i < TC_BITMASK + 1; i++)
	285	netdev_set_prio_tc_map(dev, i, qopt->prio_tc_map[i]);
	286
	287	sch->flags \|= TCQ_F_MQROOT;
	288	return 0;
b8970f0b JF	289	}
	290
	291	static void mqprio_attach(struct Qdisc *sch)
	292	{
	293	struct net_device *dev = qdisc_dev(sch);
	294	struct mqprio_sched *priv = qdisc_priv(sch);
95dc1929	295	struct Qdisc qdisc, old;
b8970f0b JF	296	unsigned int ntx;
	297
	298	/* Attach underlying qdisc */
	299	for (ntx = 0; ntx < dev->num_tx_queues; ntx++) {
	300	qdisc = priv->qdiscs[ntx];
95dc1929 ED	301	old = dev_graft_qdisc(qdisc->dev_queue, qdisc);
95dc1929 ED	302	if (old)
86bd446b	303	qdisc_put(old);
95dc1929	304	if (ntx < dev->real_num_tx_queues)
49b49971	305	qdisc_hash_add(qdisc, false);
b8970f0b JF	306	}
	307	kfree(priv->qdiscs);
	308	priv->qdiscs = NULL;
	309	}
	310
	311	static struct netdev_queue mqprio_queue_get(struct Qdisc sch,
	312	unsigned long cl)
	313	{
	314	struct net_device *dev = qdisc_dev(sch);
32302902	315	unsigned long ntx = cl - 1;
b8970f0b JF	316
	317	if (ntx >= dev->num_tx_queues)
	318	return NULL;
	319	return netdev_get_tx_queue(dev, ntx);
	320	}
	321
	322	static int mqprio_graft(struct Qdisc sch, unsigned long cl, struct Qdisc new,
653d6fd6	323	struct Qdisc *old, struct netlink_ext_ack extack)
b8970f0b JF	324	{
	325	struct net_device *dev = qdisc_dev(sch);
	326	struct netdev_queue *dev_queue = mqprio_queue_get(sch, cl);
	327
	328	if (!dev_queue)
	329	return -EINVAL;
	330
	331	if (dev->flags & IFF_UP)
	332	dev_deactivate(dev);
	333
	334	*old = dev_graft_qdisc(dev_queue, new);
	335
1abbe139	336	if (new)
4eaf3b84	337	new->flags \|= TCQ_F_ONETXQUEUE \| TCQ_F_NOPARENT;
1abbe139	338
b8970f0b JF	339	if (dev->flags & IFF_UP)
	340	dev_activate(dev);
	341
	342	return 0;
	343	}
	344
4e8b86c0 AN	345	static int dump_rates(struct mqprio_sched *priv,
	346	struct tc_mqprio_qopt opt, struct sk_buff skb)
	347	{
	348	struct nlattr *nest;
	349	int i;
	350
	351	if (priv->flags & TC_MQPRIO_F_MIN_RATE) {
	352	nest = nla_nest_start(skb, TCA_MQPRIO_MIN_RATE64);
	353	if (!nest)
	354	goto nla_put_failure;
	355
	356	for (i = 0; i < opt->num_tc; i++) {
	357	if (nla_put(skb, TCA_MQPRIO_MIN_RATE64,
	358	sizeof(priv->min_rate[i]),
	359	&priv->min_rate[i]))
	360	goto nla_put_failure;
	361	}
	362	nla_nest_end(skb, nest);
	363	}
	364
	365	if (priv->flags & TC_MQPRIO_F_MAX_RATE) {
	366	nest = nla_nest_start(skb, TCA_MQPRIO_MAX_RATE64);
	367	if (!nest)
	368	goto nla_put_failure;
	369
	370	for (i = 0; i < opt->num_tc; i++) {
	371	if (nla_put(skb, TCA_MQPRIO_MAX_RATE64,
	372	sizeof(priv->max_rate[i]),
	373	&priv->max_rate[i]))
	374	goto nla_put_failure;
	375	}
	376	nla_nest_end(skb, nest);
	377	}
	378	return 0;
	379
	380	nla_put_failure:
	381	nla_nest_cancel(skb, nest);
	382	return -1;
	383	}
	384
b8970f0b JF	385	static int mqprio_dump(struct Qdisc sch, struct sk_buff skb)
	386	{
	387	struct net_device *dev = qdisc_dev(sch);
	388	struct mqprio_sched *priv = qdisc_priv(sch);
4e8b86c0	389	struct nlattr nla = (struct nlattr )skb_tail_pointer(skb);
144ce879	390	struct tc_mqprio_qopt opt = { 0 };
b8970f0b	391	struct Qdisc *qdisc;
ce679e8d	392	unsigned int ntx, tc;
b8970f0b JF	393
	394	sch->q.qlen = 0;
	395	memset(&sch->bstats, 0, sizeof(sch->bstats));
	396	memset(&sch->qstats, 0, sizeof(sch->qstats));
	397
ce679e8d JF	398	/* MQ supports lockless qdiscs. However, statistics accounting needs
	399	* to account for all, none, or a mix of locked and unlocked child
	400	* qdiscs. Percpu stats are added to counters in-band and locking
	401	* qdisc totals are added at end.
	402	*/
	403	for (ntx = 0; ntx < dev->num_tx_queues; ntx++) {
	404	qdisc = netdev_get_tx_queue(dev, ntx)->qdisc_sleeping;
b8970f0b	405	spin_lock_bh(qdisc_lock(qdisc));
ce679e8d JF	406
	407	if (qdisc_is_percpu_stats(qdisc)) {
	408	__u32 qlen = qdisc_qlen_sum(qdisc);
	409
	410	__gnet_stats_copy_basic(NULL, &sch->bstats,
	411	qdisc->cpu_bstats,
	412	&qdisc->bstats);
	413	__gnet_stats_copy_queue(&sch->qstats,
	414	qdisc->cpu_qstats,
	415	&qdisc->qstats, qlen);
	416	} else {
	417	sch->q.qlen += qdisc->q.qlen;
	418	sch->bstats.bytes += qdisc->bstats.bytes;
	419	sch->bstats.packets += qdisc->bstats.packets;
	420	sch->qstats.backlog += qdisc->qstats.backlog;
	421	sch->qstats.drops += qdisc->qstats.drops;
	422	sch->qstats.requeues += qdisc->qstats.requeues;
	423	sch->qstats.overlimits += qdisc->qstats.overlimits;
	424	}
	425
b8970f0b JF	426	spin_unlock_bh(qdisc_lock(qdisc));
	427	}
	428
	429	opt.num_tc = netdev_get_num_tc(dev);
	430	memcpy(opt.prio_tc_map, dev->prio_tc_map, sizeof(opt.prio_tc_map));
2026fecf	431	opt.hw = priv->hw_offload;
b8970f0b	432
ce679e8d JF	433	for (tc = 0; tc < netdev_get_num_tc(dev); tc++) {
	434	opt.count[tc] = dev->tc_to_txq[tc].count;
	435	opt.offset[tc] = dev->tc_to_txq[tc].offset;
b8970f0b JF	436	}
b8970f0b JF	437
4e8b86c0 AN	438	if (nla_put(skb, TCA_OPTIONS, NLA_ALIGN(sizeof(opt)), &opt))
	439	goto nla_put_failure;
	440
	441	if ((priv->flags & TC_MQPRIO_F_MODE) &&
	442	nla_put_u16(skb, TCA_MQPRIO_MODE, priv->mode))
	443	goto nla_put_failure;
	444
	445	if ((priv->flags & TC_MQPRIO_F_SHAPER) &&
	446	nla_put_u16(skb, TCA_MQPRIO_SHAPER, priv->shaper))
	447	goto nla_put_failure;
	448
	449	if ((priv->flags & TC_MQPRIO_F_MIN_RATE \|\|
	450	priv->flags & TC_MQPRIO_F_MAX_RATE) &&
	451	(dump_rates(priv, &opt, skb) != 0))
1b34ec43	452	goto nla_put_failure;
b8970f0b	453
4e8b86c0	454	return nla_nest_end(skb, nla);
b8970f0b	455	nla_put_failure:
4e8b86c0	456	nlmsg_trim(skb, nla);
b8970f0b JF	457	return -1;
	458	}
	459
	460	static struct Qdisc mqprio_leaf(struct Qdisc sch, unsigned long cl)
	461	{
	462	struct netdev_queue *dev_queue = mqprio_queue_get(sch, cl);
	463
	464	if (!dev_queue)
	465	return NULL;
	466
	467	return dev_queue->qdisc_sleeping;
	468	}
	469
143976ce	470	static unsigned long mqprio_find(struct Qdisc *sch, u32 classid)
b8970f0b JF	471	{
	472	struct net_device *dev = qdisc_dev(sch);
	473	unsigned int ntx = TC_H_MIN(classid);
	474
32302902 AD	475	/* There are essentially two regions here that have valid classid
	476	* values. The first region will have a classid value of 1 through
	477	* num_tx_queues. All of these are backed by actual Qdiscs.
	478	*/
	479	if (ntx < TC_H_MIN_PRIORITY)
	480	return (ntx <= dev->num_tx_queues) ? ntx : 0;
	481
	482	/* The second region represents the hardware traffic classes. These
	483	* are represented by classid values of TC_H_MIN_PRIORITY through
	484	* TC_H_MIN_PRIORITY + netdev_get_num_tc - 1
	485	*/
	486	return ((ntx - TC_H_MIN_PRIORITY) < netdev_get_num_tc(dev)) ? ntx : 0;
b8970f0b JF	487	}
b8970f0b JF	488
b8970f0b JF	489	static int mqprio_dump_class(struct Qdisc *sch, unsigned long cl,
	490	struct sk_buff skb, struct tcmsg tcm)
	491	{
32302902 AD	492	if (cl < TC_H_MIN_PRIORITY) {
	493	struct netdev_queue *dev_queue = mqprio_queue_get(sch, cl);
	494	struct net_device *dev = qdisc_dev(sch);
	495	int tc = netdev_txq_to_tc(dev, cl - 1);
b8970f0b	496
32302902 AD	497	tcm->tcm_parent = (tc < 0) ? 0 :
	498	TC_H_MAKE(TC_H_MAJ(sch->handle),
	499	TC_H_MIN(tc + TC_H_MIN_PRIORITY));
	500	tcm->tcm_info = dev_queue->qdisc_sleeping->handle;
	501	} else {
b8970f0b JF	502	tcm->tcm_parent = TC_H_ROOT;
b8970f0b JF	503	tcm->tcm_info = 0;
b8970f0b JF	504	}
	505	tcm->tcm_handle \|= TC_H_MIN(cl);
	506	return 0;
	507	}
	508
	509	static int mqprio_dump_class_stats(struct Qdisc *sch, unsigned long cl,
ea18fd95	510	struct gnet_dump *d)
	511	__releases(d->lock)
	512	__acquires(d->lock)
b8970f0b	513	{
32302902	514	if (cl >= TC_H_MIN_PRIORITY) {
b8970f0b	515	int i;
64015853	516	__u32 qlen = 0;
b8970f0b JF	517	struct gnet_stats_queue qstats = {0};
b8970f0b JF	518	struct gnet_stats_basic_packed bstats = {0};
32302902 AD	519	struct net_device *dev = qdisc_dev(sch);
32302902 AD	520	struct netdev_tc_txq tc = dev->tc_to_txq[cl & TC_BITMASK];
b8970f0b JF	521
	522	/* Drop lock here it will be reclaimed before touching
	523	* statistics this is required because the d->lock we
	524	* hold here is the look on dev_queue->qdisc_sleeping
	525	* also acquired below.
	526	*/
edb09eb1 ED	527	if (d->lock)
edb09eb1 ED	528	spin_unlock_bh(d->lock);
b8970f0b JF	529
b8970f0b JF	530	for (i = tc.offset; i < tc.offset + tc.count; i++) {
46e5da40	531	struct netdev_queue *q = netdev_get_tx_queue(dev, i);
ce679e8d JF	532	struct Qdisc *qdisc = rtnl_dereference(q->qdisc);
	533	struct gnet_stats_basic_cpu __percpu *cpu_bstats = NULL;
	534	struct gnet_stats_queue __percpu *cpu_qstats = NULL;
46e5da40	535
b8970f0b	536	spin_lock_bh(qdisc_lock(qdisc));
ce679e8d JF	537	if (qdisc_is_percpu_stats(qdisc)) {
	538	cpu_bstats = qdisc->cpu_bstats;
	539	cpu_qstats = qdisc->cpu_qstats;
	540	}
	541
	542	qlen = qdisc_qlen_sum(qdisc);
	543	__gnet_stats_copy_basic(NULL, &sch->bstats,
	544	cpu_bstats, &qdisc->bstats);
	545	__gnet_stats_copy_queue(&sch->qstats,
	546	cpu_qstats,
	547	&qdisc->qstats,
	548	qlen);
b8970f0b JF	549	spin_unlock_bh(qdisc_lock(qdisc));
b8970f0b JF	550	}
ce679e8d	551
b8970f0b	552	/* Reclaim root sleeping lock before completing stats */
edb09eb1 ED	553	if (d->lock)
	554	spin_lock_bh(d->lock);
	555	if (gnet_stats_copy_basic(NULL, d, NULL, &bstats) < 0 \|\|
b0ab6f92	556	gnet_stats_copy_queue(d, NULL, &qstats, qlen) < 0)
b8970f0b JF	557	return -1;
	558	} else {
	559	struct netdev_queue *dev_queue = mqprio_queue_get(sch, cl);
	560
	561	sch = dev_queue->qdisc_sleeping;
edb09eb1 ED	562	if (gnet_stats_copy_basic(qdisc_root_sleeping_running(sch),
edb09eb1 ED	563	d, NULL, &sch->bstats) < 0 \|\|
b0ab6f92 JF	564	gnet_stats_copy_queue(d, NULL,
b0ab6f92 JF	565	&sch->qstats, sch->q.qlen) < 0)
b8970f0b JF	566	return -1;
	567	}
	568	return 0;
	569	}
	570
	571	static void mqprio_walk(struct Qdisc sch, struct qdisc_walker arg)
	572	{
	573	struct net_device *dev = qdisc_dev(sch);
	574	unsigned long ntx;
	575
	576	if (arg->stop)
	577	return;
	578
	579	/* Walk hierarchy with a virtual class per tc */
	580	arg->count = arg->skip;
32302902 AD	581	for (ntx = arg->skip; ntx < netdev_get_num_tc(dev); ntx++) {
	582	if (arg->fn(sch, ntx + TC_H_MIN_PRIORITY, arg) < 0) {
	583	arg->stop = 1;
	584	return;
	585	}
	586	arg->count++;
	587	}
	588
	589	/* Pad the values and skip over unused traffic classes */
	590	if (ntx < TC_MAX_QUEUE) {
	591	arg->count = TC_MAX_QUEUE;
	592	ntx = TC_MAX_QUEUE;
	593	}
	594
	595	/* Reset offset, sort out remaining per-queue qdiscs */
	596	for (ntx -= TC_MAX_QUEUE; ntx < dev->num_tx_queues; ntx++) {
b8970f0b JF	597	if (arg->fn(sch, ntx + 1, arg) < 0) {
b8970f0b JF	598	arg->stop = 1;
32302902	599	return;
b8970f0b JF	600	}
	601	arg->count++;
	602	}
	603	}
	604
0f7787b4 JSP	605	static struct netdev_queue mqprio_select_queue(struct Qdisc sch,
	606	struct tcmsg *tcm)
	607	{
	608	return mqprio_queue_get(sch, TC_H_MIN(tcm->tcm_parent));
	609	}
	610
b8970f0b JF	611	static const struct Qdisc_class_ops mqprio_class_ops = {
	612	.graft = mqprio_graft,
	613	.leaf = mqprio_leaf,
143976ce	614	.find = mqprio_find,
b8970f0b JF	615	.walk = mqprio_walk,
	616	.dump = mqprio_dump_class,
	617	.dump_stats = mqprio_dump_class_stats,
0f7787b4	618	.select_queue = mqprio_select_queue,
b8970f0b JF	619	};
b8970f0b JF	620
ea18fd95	621	static struct Qdisc_ops mqprio_qdisc_ops __read_mostly = {
b8970f0b JF	622	.cl_ops = &mqprio_class_ops,
	623	.id = "mqprio",
	624	.priv_size = sizeof(struct mqprio_sched),
	625	.init = mqprio_init,
	626	.destroy = mqprio_destroy,
	627	.attach = mqprio_attach,
	628	.dump = mqprio_dump,
	629	.owner = THIS_MODULE,
	630	};
	631
	632	static int __init mqprio_module_init(void)
	633	{
	634	return register_qdisc(&mqprio_qdisc_ops);
	635	}
	636
	637	static void __exit mqprio_module_exit(void)
	638	{
	639	unregister_qdisc(&mqprio_qdisc_ops);
	640	}
	641
	642	module_init(mqprio_module_init);
	643	module_exit(mqprio_module_exit);
	644
	645	MODULE_LICENSE("GPL");