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

// SPDX-License-Identifier: GPL-2.0-only
/* Flow Queue PIE discipline
 *
 * Copyright (C) 2019 Mohit P. Tahiliani <tahiliani@nitk.edu.in>
 * Copyright (C) 2019 Sachin D. Patil <sdp.sachin@gmail.com>
 * Copyright (C) 2019 V. Saicharan <vsaicharan1998@gmail.com>
 * Copyright (C) 2019 Mohit Bhasi <mohitbhasi1998@gmail.com>
 * Copyright (C) 2019 Leslie Monis <lesliemonis@gmail.com>
 * Copyright (C) 2019 Gautam Ramakrishnan <gautamramk@gmail.com>
 */

#include <linux/jhash.h>
#include <linux/sizes.h>
#include <linux/vmalloc.h>
#include <net/pkt_cls.h>
#include <net/pie.h>

/* Flow Queue PIE
 *
 * Principles:
 *   - Packets are classified on flows.
 *   - This is a Stochastic model (as we use a hash, several flows might
 *                                 be hashed to the same slot)
 *   - Each flow has a PIE managed queue.
 *   - Flows are linked onto two (Round Robin) lists,
 *     so that new flows have priority on old ones.
 *   - For a given flow, packets are not reordered.
 *   - Drops during enqueue only.
 *   - ECN capability is off by default.
 *   - ECN threshold (if ECN is enabled) is at 10% by default.
 *   - Uses timestamps to calculate queue delay by default.
 */

/**
 * struct fq_pie_flow - contains data for each flow
 * @vars:	pie vars associated with the flow
 * @deficit:	number of remaining byte credits
 * @backlog:	size of data in the flow
 * @qlen:	number of packets in the flow
 * @flowchain:	flowchain for the flow
 * @head:	first packet in the flow
 * @tail:	last packet in the flow
 */
struct fq_pie_flow {
	struct pie_vars vars;
	s32 deficit;
	u32 backlog;
	u32 qlen;
	struct list_head flowchain;
	struct sk_buff *head;
	struct sk_buff *tail;
};

struct fq_pie_sched_data {
	struct tcf_proto __rcu *filter_list; /* optional external classifier */
	struct tcf_block *block;
	struct fq_pie_flow *flows;
	struct Qdisc *sch;
	struct list_head old_flows;
	struct list_head new_flows;
	struct pie_params p_params;
	u32 ecn_prob;
	u32 flows_cnt;
	u32 quantum;
	u32 memory_limit;
	u32 new_flow_count;
	u32 memory_usage;
	u32 overmemory;
	struct pie_stats stats;
	struct timer_list adapt_timer;
};

static unsigned int fq_pie_hash(const struct fq_pie_sched_data *q,
				struct sk_buff *skb)
{
	return reciprocal_scale(skb_get_hash(skb), q->flows_cnt);
}

static unsigned int fq_pie_classify(struct sk_buff *skb, struct Qdisc *sch,
				    int *qerr)
{
	struct fq_pie_sched_data *q = qdisc_priv(sch);
	struct tcf_proto *filter;
	struct tcf_result res;
	int result;

	if (TC_H_MAJ(skb->priority) == sch->handle &&
	    TC_H_MIN(skb->priority) > 0 &&
	    TC_H_MIN(skb->priority) <= q->flows_cnt)
		return TC_H_MIN(skb->priority);

	filter = rcu_dereference_bh(q->filter_list);
	if (!filter)
		return fq_pie_hash(q, skb) + 1;

	*qerr = NET_XMIT_SUCCESS | __NET_XMIT_BYPASS;
	result = tcf_classify(skb, NULL, filter, &res, false);
	if (result >= 0) {
#ifdef CONFIG_NET_CLS_ACT
		switch (result) {
		case TC_ACT_STOLEN:
		case TC_ACT_QUEUED:
		case TC_ACT_TRAP:
			*qerr = NET_XMIT_SUCCESS | __NET_XMIT_STOLEN;
			fallthrough;
		case TC_ACT_SHOT:
			return 0;
		}
#endif
		if (TC_H_MIN(res.classid) <= q->flows_cnt)
			return TC_H_MIN(res.classid);
	}
	return 0;
}

/* add skb to flow queue (tail add) */
static inline void flow_queue_add(struct fq_pie_flow *flow,
				  struct sk_buff *skb)
{
	if (!flow->head)
		flow->head = skb;
	else
		flow->tail->next = skb;
	flow->tail = skb;
	skb->next = NULL;
}

static int fq_pie_qdisc_enqueue(struct sk_buff *skb, struct Qdisc *sch,
				struct sk_buff **to_free)
{
	struct fq_pie_sched_data *q = qdisc_priv(sch);
	struct fq_pie_flow *sel_flow;
	int ret;
	u8 memory_limited = false;
	u8 enqueue = false;
	u32 pkt_len;
	u32 idx;

	/* Classifies packet into corresponding flow */
	idx = fq_pie_classify(skb, sch, &ret);
	if (idx == 0) {
		if (ret & __NET_XMIT_BYPASS)
			qdisc_qstats_drop(sch);
		__qdisc_drop(skb, to_free);
		return ret;
	}
	idx--;

	sel_flow = &q->flows[idx];
	/* Checks whether adding a new packet would exceed memory limit */
	get_pie_cb(skb)->mem_usage = skb->truesize;
	memory_limited = q->memory_usage > q->memory_limit + skb->truesize;

	/* Checks if the qdisc is full */
	if (unlikely(qdisc_qlen(sch) >= sch->limit)) {
		q->stats.overlimit++;
		goto out;
	} else if (unlikely(memory_limited)) {
		q->overmemory++;
	}

	if (!pie_drop_early(sch, &q->p_params, &sel_flow->vars,
			    sel_flow->backlog, skb->len)) {
		enqueue = true;
	} else if (q->p_params.ecn &&
		   sel_flow->vars.prob <= (MAX_PROB / 100) * q->ecn_prob &&
		   INET_ECN_set_ce(skb)) {
		/* If packet is ecn capable, mark it if drop probability
		 * is lower than the parameter ecn_prob, else drop it.
		 */
		q->stats.ecn_mark++;
		enqueue = true;
	}
	if (enqueue) {
		/* Set enqueue time only when dq_rate_estimator is disabled. */
		if (!q->p_params.dq_rate_estimator)
			pie_set_enqueue_time(skb);

		pkt_len = qdisc_pkt_len(skb);
		q->stats.packets_in++;
		q->memory_usage += skb->truesize;
		sch->qstats.backlog += pkt_len;
		sch->q.qlen++;
		flow_queue_add(sel_flow, skb);
		if (list_empty(&sel_flow->flowchain)) {
			list_add_tail(&sel_flow->flowchain, &q->new_flows);
			q->new_flow_count++;
			sel_flow->deficit = q->quantum;
			sel_flow->qlen = 0;
			sel_flow->backlog = 0;
		}
		sel_flow->qlen++;
		sel_flow->backlog += pkt_len;
		return NET_XMIT_SUCCESS;
	}
out:
	q->stats.dropped++;
	sel_flow->vars.accu_prob = 0;
	__qdisc_drop(skb, to_free);
	qdisc_qstats_drop(sch);
	return NET_XMIT_CN;
}

static const struct nla_policy fq_pie_policy[TCA_FQ_PIE_MAX + 1] = {
	[TCA_FQ_PIE_LIMIT]		= {.type = NLA_U32},
	[TCA_FQ_PIE_FLOWS]		= {.type = NLA_U32},
	[TCA_FQ_PIE_TARGET]		= {.type = NLA_U32},
	[TCA_FQ_PIE_TUPDATE]		= {.type = NLA_U32},
	[TCA_FQ_PIE_ALPHA]		= {.type = NLA_U32},
	[TCA_FQ_PIE_BETA]		= {.type = NLA_U32},
	[TCA_FQ_PIE_QUANTUM]		= {.type = NLA_U32},
	[TCA_FQ_PIE_MEMORY_LIMIT]	= {.type = NLA_U32},
	[TCA_FQ_PIE_ECN_PROB]		= {.type = NLA_U32},
	[TCA_FQ_PIE_ECN]		= {.type = NLA_U32},
	[TCA_FQ_PIE_BYTEMODE]		= {.type = NLA_U32},
	[TCA_FQ_PIE_DQ_RATE_ESTIMATOR]	= {.type = NLA_U32},
};

static inline struct sk_buff *dequeue_head(struct fq_pie_flow *flow)
{
	struct sk_buff *skb = flow->head;

	flow->head = skb->next;
	skb->next = NULL;
	return skb;
}

static struct sk_buff *fq_pie_qdisc_dequeue(struct Qdisc *sch)
{
	struct fq_pie_sched_data *q = qdisc_priv(sch);
	struct sk_buff *skb = NULL;
	struct fq_pie_flow *flow;
	struct list_head *head;
	u32 pkt_len;

begin:
	head = &q->new_flows;
	if (list_empty(head)) {
		head = &q->old_flows;
		if (list_empty(head))
			return NULL;
	}

	flow = list_first_entry(head, struct fq_pie_flow, flowchain);
	/* Flow has exhausted all its credits */
	if (flow->deficit <= 0) {
		flow->deficit += q->quantum;
		list_move_tail(&flow->flowchain, &q->old_flows);
		goto begin;
	}

	if (flow->head) {
		skb = dequeue_head(flow);
		pkt_len = qdisc_pkt_len(skb);
		sch->qstats.backlog -= pkt_len;
		sch->q.qlen--;
		qdisc_bstats_update(sch, skb);
	}

	if (!skb) {
		/* force a pass through old_flows to prevent starvation */
		if (head == &q->new_flows && !list_empty(&q->old_flows))
			list_move_tail(&flow->flowchain, &q->old_flows);
		else
			list_del_init(&flow->flowchain);
		goto begin;
	}

	flow->qlen--;
	flow->deficit -= pkt_len;
	flow->backlog -= pkt_len;
	q->memory_usage -= get_pie_cb(skb)->mem_usage;
	pie_process_dequeue(skb, &q->p_params, &flow->vars, flow->backlog);
	return skb;
}

static int fq_pie_change(struct Qdisc *sch, struct nlattr *opt,
			 struct netlink_ext_ack *extack)
{
	struct fq_pie_sched_data *q = qdisc_priv(sch);
	struct nlattr *tb[TCA_FQ_PIE_MAX + 1];
	unsigned int len_dropped = 0;
	unsigned int num_dropped = 0;
	int err;

	err = nla_parse_nested(tb, TCA_FQ_PIE_MAX, opt, fq_pie_policy, extack);
	if (err < 0)
		return err;

	sch_tree_lock(sch);
	if (tb[TCA_FQ_PIE_LIMIT]) {
		u32 limit = nla_get_u32(tb[TCA_FQ_PIE_LIMIT]);

		q->p_params.limit = limit;
		sch->limit = limit;
	}
	if (tb[TCA_FQ_PIE_FLOWS]) {
		if (q->flows) {
			NL_SET_ERR_MSG_MOD(extack,
					   "Number of flows cannot be changed");
			goto flow_error;
		}
		q->flows_cnt = nla_get_u32(tb[TCA_FQ_PIE_FLOWS]);
		if (!q->flows_cnt || q->flows_cnt > 65536) {
			NL_SET_ERR_MSG_MOD(extack,
					   "Number of flows must range in [1..65536]");
			goto flow_error;
		}
	}

	/* convert from microseconds to pschedtime */
	if (tb[TCA_FQ_PIE_TARGET]) {
		/* target is in us */
		u32 target = nla_get_u32(tb[TCA_FQ_PIE_TARGET]);

		/* convert to pschedtime */
		q->p_params.target =
			PSCHED_NS2TICKS((u64)target * NSEC_PER_USEC);
	}

	/* tupdate is in jiffies */
	if (tb[TCA_FQ_PIE_TUPDATE])
		q->p_params.tupdate =
			usecs_to_jiffies(nla_get_u32(tb[TCA_FQ_PIE_TUPDATE]));

	if (tb[TCA_FQ_PIE_ALPHA])
		q->p_params.alpha = nla_get_u32(tb[TCA_FQ_PIE_ALPHA]);

	if (tb[TCA_FQ_PIE_BETA])
		q->p_params.beta = nla_get_u32(tb[TCA_FQ_PIE_BETA]);

	if (tb[TCA_FQ_PIE_QUANTUM])
		q->quantum = nla_get_u32(tb[TCA_FQ_PIE_QUANTUM]);

	if (tb[TCA_FQ_PIE_MEMORY_LIMIT])
		q->memory_limit = nla_get_u32(tb[TCA_FQ_PIE_MEMORY_LIMIT]);

	if (tb[TCA_FQ_PIE_ECN_PROB])
		q->ecn_prob = nla_get_u32(tb[TCA_FQ_PIE_ECN_PROB]);

	if (tb[TCA_FQ_PIE_ECN])
		q->p_params.ecn = nla_get_u32(tb[TCA_FQ_PIE_ECN]);

	if (tb[TCA_FQ_PIE_BYTEMODE])
		q->p_params.bytemode = nla_get_u32(tb[TCA_FQ_PIE_BYTEMODE]);

	if (tb[TCA_FQ_PIE_DQ_RATE_ESTIMATOR])
		q->p_params.dq_rate_estimator =
			nla_get_u32(tb[TCA_FQ_PIE_DQ_RATE_ESTIMATOR]);

	/* Drop excess packets if new limit is lower */
	while (sch->q.qlen > sch->limit) {
		struct sk_buff *skb = fq_pie_qdisc_dequeue(sch);

		len_dropped += qdisc_pkt_len(skb);
		num_dropped += 1;
		rtnl_kfree_skbs(skb, skb);
	}
	qdisc_tree_reduce_backlog(sch, num_dropped, len_dropped);

	sch_tree_unlock(sch);
	return 0;

flow_error:
	sch_tree_unlock(sch);
	return -EINVAL;
}

static void fq_pie_timer(struct timer_list *t)
{
	struct fq_pie_sched_data *q = from_timer(q, t, adapt_timer);
	struct Qdisc *sch = q->sch;
	spinlock_t *root_lock; /* to lock qdisc for probability calculations */
	u32 idx;

	root_lock = qdisc_lock(qdisc_root_sleeping(sch));
	spin_lock(root_lock);

	for (idx = 0; idx < q->flows_cnt; idx++)
		pie_calculate_probability(&q->p_params, &q->flows[idx].vars,
					  q->flows[idx].backlog);

	/* reset the timer to fire after 'tupdate' jiffies. */
	if (q->p_params.tupdate)
		mod_timer(&q->adapt_timer, jiffies + q->p_params.tupdate);

	spin_unlock(root_lock);
}

static int fq_pie_init(struct Qdisc *sch, struct nlattr *opt,
		       struct netlink_ext_ack *extack)
{
	struct fq_pie_sched_data *q = qdisc_priv(sch);
	int err;
	u32 idx;

	pie_params_init(&q->p_params);
	sch->limit = 10 * 1024;
	q->p_params.limit = sch->limit;
	q->quantum = psched_mtu(qdisc_dev(sch));
	q->sch = sch;
	q->ecn_prob = 10;
	q->flows_cnt = 1024;
	q->memory_limit = SZ_32M;

	INIT_LIST_HEAD(&q->new_flows);
	INIT_LIST_HEAD(&q->old_flows);
	timer_setup(&q->adapt_timer, fq_pie_timer, 0);

	if (opt) {
		err = fq_pie_change(sch, opt, extack);

		if (err)
			return err;
	}

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

	q->flows = kvcalloc(q->flows_cnt, sizeof(struct fq_pie_flow),
			    GFP_KERNEL);
	if (!q->flows) {
		err = -ENOMEM;
		goto init_failure;
	}
	for (idx = 0; idx < q->flows_cnt; idx++) {
		struct fq_pie_flow *flow = q->flows + idx;

		INIT_LIST_HEAD(&flow->flowchain);
		pie_vars_init(&flow->vars);
	}

	mod_timer(&q->adapt_timer, jiffies + HZ / 2);

	return 0;

init_failure:
	q->flows_cnt = 0;

	return err;
}

static int fq_pie_dump(struct Qdisc *sch, struct sk_buff *skb)
{
	struct fq_pie_sched_data *q = qdisc_priv(sch);
	struct nlattr *opts;

	opts = nla_nest_start(skb, TCA_OPTIONS);
	if (!opts)
		return -EMSGSIZE;

	/* convert target from pschedtime to us */
	if (nla_put_u32(skb, TCA_FQ_PIE_LIMIT, sch->limit) ||
	    nla_put_u32(skb, TCA_FQ_PIE_FLOWS, q->flows_cnt) ||
	    nla_put_u32(skb, TCA_FQ_PIE_TARGET,
			((u32)PSCHED_TICKS2NS(q->p_params.target)) /
			NSEC_PER_USEC) ||
	    nla_put_u32(skb, TCA_FQ_PIE_TUPDATE,
			jiffies_to_usecs(q->p_params.tupdate)) ||
	    nla_put_u32(skb, TCA_FQ_PIE_ALPHA, q->p_params.alpha) ||
	    nla_put_u32(skb, TCA_FQ_PIE_BETA, q->p_params.beta) ||
	    nla_put_u32(skb, TCA_FQ_PIE_QUANTUM, q->quantum) ||
	    nla_put_u32(skb, TCA_FQ_PIE_MEMORY_LIMIT, q->memory_limit) ||
	    nla_put_u32(skb, TCA_FQ_PIE_ECN_PROB, q->ecn_prob) ||
	    nla_put_u32(skb, TCA_FQ_PIE_ECN, q->p_params.ecn) ||
	    nla_put_u32(skb, TCA_FQ_PIE_BYTEMODE, q->p_params.bytemode) ||
	    nla_put_u32(skb, TCA_FQ_PIE_DQ_RATE_ESTIMATOR,
			q->p_params.dq_rate_estimator))
		goto nla_put_failure;

	return nla_nest_end(skb, opts);

nla_put_failure:
	nla_nest_cancel(skb, opts);
	return -EMSGSIZE;
}

static int fq_pie_dump_stats(struct Qdisc *sch, struct gnet_dump *d)
{
	struct fq_pie_sched_data *q = qdisc_priv(sch);
	struct tc_fq_pie_xstats st = {
		.packets_in	= q->stats.packets_in,
		.overlimit	= q->stats.overlimit,
		.overmemory	= q->overmemory,
		.dropped	= q->stats.dropped,
		.ecn_mark	= q->stats.ecn_mark,
		.new_flow_count = q->new_flow_count,
		.memory_usage   = q->memory_usage,
	};
	struct list_head *pos;

	sch_tree_lock(sch);
	list_for_each(pos, &q->new_flows)
		st.new_flows_len++;

	list_for_each(pos, &q->old_flows)
		st.old_flows_len++;
	sch_tree_unlock(sch);

	return gnet_stats_copy_app(d, &st, sizeof(st));
}

static void fq_pie_reset(struct Qdisc *sch)
{
	struct fq_pie_sched_data *q = qdisc_priv(sch);
	u32 idx;

	INIT_LIST_HEAD(&q->new_flows);
	INIT_LIST_HEAD(&q->old_flows);
	for (idx = 0; idx < q->flows_cnt; idx++) {
		struct fq_pie_flow *flow = q->flows + idx;

		/* Removes all packets from flow */
		rtnl_kfree_skbs(flow->head, flow->tail);
		flow->head = NULL;

		INIT_LIST_HEAD(&flow->flowchain);
		pie_vars_init(&flow->vars);
	}
}

static void fq_pie_destroy(struct Qdisc *sch)
{
	struct fq_pie_sched_data *q = qdisc_priv(sch);

	tcf_block_put(q->block);
	q->p_params.tupdate = 0;
	del_timer_sync(&q->adapt_timer);
	kvfree(q->flows);
}

static struct Qdisc_ops fq_pie_qdisc_ops __read_mostly = {
	.id		= "fq_pie",
	.priv_size	= sizeof(struct fq_pie_sched_data),
	.enqueue	= fq_pie_qdisc_enqueue,
	.dequeue	= fq_pie_qdisc_dequeue,
	.peek		= qdisc_peek_dequeued,
	.init		= fq_pie_init,
	.destroy	= fq_pie_destroy,
	.reset		= fq_pie_reset,
	.change		= fq_pie_change,
	.dump		= fq_pie_dump,
	.dump_stats	= fq_pie_dump_stats,
	.owner		= THIS_MODULE,
};

static int __init fq_pie_module_init(void)
{
	return register_qdisc(&fq_pie_qdisc_ops);
}

static void __exit fq_pie_module_exit(void)
{
	unregister_qdisc(&fq_pie_qdisc_ops);
}

module_init(fq_pie_module_init);
module_exit(fq_pie_module_exit);

MODULE_DESCRIPTION("Flow Queue Proportional Integral controller Enhanced (FQ-PIE)");
MODULE_AUTHOR("Mohit P. Tahiliani");
MODULE_LICENSE("GPL");
Commit	Line	Data
ec97ecf1 MT	1	// SPDX-License-Identifier: GPL-2.0-only
	2	/* Flow Queue PIE discipline
	3	*
	4	* Copyright (C) 2019 Mohit P. Tahiliani <tahiliani@nitk.edu.in>
	5	* Copyright (C) 2019 Sachin D. Patil <sdp.sachin@gmail.com>
	6	* Copyright (C) 2019 V. Saicharan <vsaicharan1998@gmail.com>
	7	* Copyright (C) 2019 Mohit Bhasi <mohitbhasi1998@gmail.com>
	8	* Copyright (C) 2019 Leslie Monis <lesliemonis@gmail.com>
	9	* Copyright (C) 2019 Gautam Ramakrishnan <gautamramk@gmail.com>
	10	*/
	11
	12	#include <linux/jhash.h>
	13	#include <linux/sizes.h>
	14	#include <linux/vmalloc.h>
	15	#include <net/pkt_cls.h>
	16	#include <net/pie.h>
	17
	18	/* Flow Queue PIE
	19	*
	20	* Principles:
	21	* - Packets are classified on flows.
	22	* - This is a Stochastic model (as we use a hash, several flows might
	23	* be hashed to the same slot)
	24	* - Each flow has a PIE managed queue.
	25	* - Flows are linked onto two (Round Robin) lists,
	26	* so that new flows have priority on old ones.
	27	* - For a given flow, packets are not reordered.
	28	* - Drops during enqueue only.
	29	* - ECN capability is off by default.
	30	* - ECN threshold (if ECN is enabled) is at 10% by default.
	31	* - Uses timestamps to calculate queue delay by default.
	32	*/
	33
	34	/**
	35	* struct fq_pie_flow - contains data for each flow
	36	* @vars: pie vars associated with the flow
	37	* @deficit: number of remaining byte credits
	38	* @backlog: size of data in the flow
	39	* @qlen: number of packets in the flow
	40	* @flowchain: flowchain for the flow
	41	* @head: first packet in the flow
	42	* @tail: last packet in the flow
	43	*/
	44	struct fq_pie_flow {
	45	struct pie_vars vars;
	46	s32 deficit;
	47	u32 backlog;
	48	u32 qlen;
	49	struct list_head flowchain;
	50	struct sk_buff *head;
	51	struct sk_buff *tail;
	52	};
	53
	54	struct fq_pie_sched_data {
	55	struct tcf_proto __rcu filter_list; / optional external classifier */
	56	struct tcf_block *block;
	57	struct fq_pie_flow *flows;
	58	struct Qdisc *sch;
	59	struct list_head old_flows;
	60	struct list_head new_flows;
	61	struct pie_params p_params;
	62	u32 ecn_prob;
	63	u32 flows_cnt;
	64	u32 quantum;
65	u32 memory_limit;
66	u32 new_flow_count;
67	u32 memory_usage;
68	u32 overmemory;
69	struct pie_stats stats;
70	struct timer_list adapt_timer;
71	};
72
73	static unsigned int fq_pie_hash(const struct fq_pie_sched_data *q,
74	struct sk_buff *skb)
75	{
76	return reciprocal_scale(skb_get_hash(skb), q->flows_cnt);
77	}
78
79	static unsigned int fq_pie_classify(struct sk_buff skb, struct Qdisc sch,
80	int *qerr)
81	{
82	struct fq_pie_sched_data *q = qdisc_priv(sch);
83	struct tcf_proto *filter;
84	struct tcf_result res;
85	int result;
86
87	if (TC_H_MAJ(skb->priority) == sch->handle &&
88	TC_H_MIN(skb->priority) > 0 &&
89	TC_H_MIN(skb->priority) <= q->flows_cnt)
90	return TC_H_MIN(skb->priority);
91
92	filter = rcu_dereference_bh(q->filter_list);
93	if (!filter)
94	return fq_pie_hash(q, skb) + 1;
95
96	*qerr = NET_XMIT_SUCCESS \| __NET_XMIT_BYPASS;
3aa26055	97	result = tcf_classify(skb, NULL, filter, &res, false);
ec97ecf1 MT	98	if (result >= 0) {
	99	#ifdef CONFIG_NET_CLS_ACT
	100	switch (result) {
	101	case TC_ACT_STOLEN:
	102	case TC_ACT_QUEUED:
	103	case TC_ACT_TRAP:
	104	*qerr = NET_XMIT_SUCCESS \| __NET_XMIT_STOLEN;
964201de	105	fallthrough;
ec97ecf1 MT	106	case TC_ACT_SHOT:
	107	return 0;
	108	}
	109	#endif
	110	if (TC_H_MIN(res.classid) <= q->flows_cnt)
	111	return TC_H_MIN(res.classid);
	112	}
	113	return 0;
	114	}
	115
	116	/* add skb to flow queue (tail add) */
	117	static inline void flow_queue_add(struct fq_pie_flow *flow,
	118	struct sk_buff *skb)
	119	{
	120	if (!flow->head)
	121	flow->head = skb;
	122	else
	123	flow->tail->next = skb;
	124	flow->tail = skb;
	125	skb->next = NULL;
	126	}
	127
	128	static int fq_pie_qdisc_enqueue(struct sk_buff skb, struct Qdisc sch,
	129	struct sk_buff **to_free)
	130	{
	131	struct fq_pie_sched_data *q = qdisc_priv(sch);
	132	struct fq_pie_flow *sel_flow;
3f649ab7	133	int ret;
ec97ecf1 MT	134	u8 memory_limited = false;
	135	u8 enqueue = false;
	136	u32 pkt_len;
	137	u32 idx;
	138
	139	/* Classifies packet into corresponding flow */
	140	idx = fq_pie_classify(skb, sch, &ret);
e70f7a11 DC	141	if (idx == 0) {
	142	if (ret & __NET_XMIT_BYPASS)
	143	qdisc_qstats_drop(sch);
	144	__qdisc_drop(skb, to_free);
	145	return ret;
	146	}
	147	idx--;
ec97ecf1	148
e70f7a11	149	sel_flow = &q->flows[idx];
ec97ecf1 MT	150	/* Checks whether adding a new packet would exceed memory limit */
	151	get_pie_cb(skb)->mem_usage = skb->truesize;
	152	memory_limited = q->memory_usage > q->memory_limit + skb->truesize;
	153
	154	/* Checks if the qdisc is full */
	155	if (unlikely(qdisc_qlen(sch) >= sch->limit)) {
	156	q->stats.overlimit++;
	157	goto out;
	158	} else if (unlikely(memory_limited)) {
	159	q->overmemory++;
	160	}
	161
	162	if (!pie_drop_early(sch, &q->p_params, &sel_flow->vars,
	163	sel_flow->backlog, skb->len)) {
	164	enqueue = true;
	165	} else if (q->p_params.ecn &&
	166	sel_flow->vars.prob <= (MAX_PROB / 100) * q->ecn_prob &&
	167	INET_ECN_set_ce(skb)) {
	168	/* If packet is ecn capable, mark it if drop probability
	169	* is lower than the parameter ecn_prob, else drop it.
	170	*/
	171	q->stats.ecn_mark++;
	172	enqueue = true;
	173	}
	174	if (enqueue) {
	175	/* Set enqueue time only when dq_rate_estimator is disabled. */
	176	if (!q->p_params.dq_rate_estimator)
	177	pie_set_enqueue_time(skb);
	178
	179	pkt_len = qdisc_pkt_len(skb);
	180	q->stats.packets_in++;
	181	q->memory_usage += skb->truesize;
	182	sch->qstats.backlog += pkt_len;
	183	sch->q.qlen++;
	184	flow_queue_add(sel_flow, skb);
	185	if (list_empty(&sel_flow->flowchain)) {
	186	list_add_tail(&sel_flow->flowchain, &q->new_flows);
	187	q->new_flow_count++;
	188	sel_flow->deficit = q->quantum;
	189	sel_flow->qlen = 0;
	190	sel_flow->backlog = 0;
	191	}
	192	sel_flow->qlen++;
	193	sel_flow->backlog += pkt_len;
	194	return NET_XMIT_SUCCESS;
	195	}
	196	out:
	197	q->stats.dropped++;
	198	sel_flow->vars.accu_prob = 0;
ec97ecf1 MT	199	__qdisc_drop(skb, to_free);
	200	qdisc_qstats_drop(sch);
	201	return NET_XMIT_CN;
	202	}
	203
	204	static const struct nla_policy fq_pie_policy[TCA_FQ_PIE_MAX + 1] = {
	205	[TCA_FQ_PIE_LIMIT] = {.type = NLA_U32},
	206	[TCA_FQ_PIE_FLOWS] = {.type = NLA_U32},
	207	[TCA_FQ_PIE_TARGET] = {.type = NLA_U32},
	208	[TCA_FQ_PIE_TUPDATE] = {.type = NLA_U32},
	209	[TCA_FQ_PIE_ALPHA] = {.type = NLA_U32},
	210	[TCA_FQ_PIE_BETA] = {.type = NLA_U32},
	211	[TCA_FQ_PIE_QUANTUM] = {.type = NLA_U32},
	212	[TCA_FQ_PIE_MEMORY_LIMIT] = {.type = NLA_U32},
	213	[TCA_FQ_PIE_ECN_PROB] = {.type = NLA_U32},
	214	[TCA_FQ_PIE_ECN] = {.type = NLA_U32},
	215	[TCA_FQ_PIE_BYTEMODE] = {.type = NLA_U32},
	216	[TCA_FQ_PIE_DQ_RATE_ESTIMATOR] = {.type = NLA_U32},
	217	};
	218
	219	static inline struct sk_buff dequeue_head(struct fq_pie_flow flow)
	220	{
	221	struct sk_buff *skb = flow->head;
	222
	223	flow->head = skb->next;
	224	skb->next = NULL;
	225	return skb;
	226	}
	227
	228	static struct sk_buff fq_pie_qdisc_dequeue(struct Qdisc sch)
	229	{
	230	struct fq_pie_sched_data *q = qdisc_priv(sch);
	231	struct sk_buff *skb = NULL;
	232	struct fq_pie_flow *flow;
	233	struct list_head *head;
	234	u32 pkt_len;
	235
	236	begin:
	237	head = &q->new_flows;
	238	if (list_empty(head)) {
	239	head = &q->old_flows;
	240	if (list_empty(head))
	241	return NULL;
	242	}
	243
	244	flow = list_first_entry(head, struct fq_pie_flow, flowchain);
	245	/* Flow has exhausted all its credits */
	246	if (flow->deficit <= 0) {
	247	flow->deficit += q->quantum;
	248	list_move_tail(&flow->flowchain, &q->old_flows);
	249	goto begin;
	250	}
	251
	252	if (flow->head) {
	253	skb = dequeue_head(flow);
	254	pkt_len = qdisc_pkt_len(skb);
	255	sch->qstats.backlog -= pkt_len;
	256	sch->q.qlen--;
	257	qdisc_bstats_update(sch, skb);
	258	}
	259
	260	if (!skb) {
	261	/* force a pass through old_flows to prevent starvation */
	262	if (head == &q->new_flows && !list_empty(&q->old_flows))
263	list_move_tail(&flow->flowchain, &q->old_flows);
264	else
265	list_del_init(&flow->flowchain);
266	goto begin;
267	}
268
269	flow->qlen--;
270	flow->deficit -= pkt_len;
271	flow->backlog -= pkt_len;
272	q->memory_usage -= get_pie_cb(skb)->mem_usage;
273	pie_process_dequeue(skb, &q->p_params, &flow->vars, flow->backlog);
274	return skb;
275	}
276
277	static int fq_pie_change(struct Qdisc sch, struct nlattr opt,
278	struct netlink_ext_ack *extack)
279	{
280	struct fq_pie_sched_data *q = qdisc_priv(sch);
281	struct nlattr *tb[TCA_FQ_PIE_MAX + 1];
282	unsigned int len_dropped = 0;
283	unsigned int num_dropped = 0;
284	int err;
285
ec97ecf1 MT	286	err = nla_parse_nested(tb, TCA_FQ_PIE_MAX, opt, fq_pie_policy, extack);
	287	if (err < 0)
	288	return err;
	289
	290	sch_tree_lock(sch);
	291	if (tb[TCA_FQ_PIE_LIMIT]) {
	292	u32 limit = nla_get_u32(tb[TCA_FQ_PIE_LIMIT]);
	293
	294	q->p_params.limit = limit;
	295	sch->limit = limit;
	296	}
	297	if (tb[TCA_FQ_PIE_FLOWS]) {
	298	if (q->flows) {
	299	NL_SET_ERR_MSG_MOD(extack,
	300	"Number of flows cannot be changed");
	301	goto flow_error;
	302	}
	303	q->flows_cnt = nla_get_u32(tb[TCA_FQ_PIE_FLOWS]);
3a62fed2	304	if (!q->flows_cnt \|\| q->flows_cnt > 65536) {
ec97ecf1	305	NL_SET_ERR_MSG_MOD(extack,
3a62fed2	306	"Number of flows must range in [1..65536]");
ec97ecf1 MT	307	goto flow_error;
	308	}
	309	}
	310
	311	/* convert from microseconds to pschedtime */
	312	if (tb[TCA_FQ_PIE_TARGET]) {
	313	/* target is in us */
	314	u32 target = nla_get_u32(tb[TCA_FQ_PIE_TARGET]);
	315
	316	/* convert to pschedtime */
	317	q->p_params.target =
	318	PSCHED_NS2TICKS((u64)target * NSEC_PER_USEC);
	319	}
	320
	321	/* tupdate is in jiffies */
	322	if (tb[TCA_FQ_PIE_TUPDATE])
	323	q->p_params.tupdate =
	324	usecs_to_jiffies(nla_get_u32(tb[TCA_FQ_PIE_TUPDATE]));
	325
	326	if (tb[TCA_FQ_PIE_ALPHA])
	327	q->p_params.alpha = nla_get_u32(tb[TCA_FQ_PIE_ALPHA]);
	328
	329	if (tb[TCA_FQ_PIE_BETA])
	330	q->p_params.beta = nla_get_u32(tb[TCA_FQ_PIE_BETA]);
	331
	332	if (tb[TCA_FQ_PIE_QUANTUM])
	333	q->quantum = nla_get_u32(tb[TCA_FQ_PIE_QUANTUM]);
	334
	335	if (tb[TCA_FQ_PIE_MEMORY_LIMIT])
	336	q->memory_limit = nla_get_u32(tb[TCA_FQ_PIE_MEMORY_LIMIT]);
	337
	338	if (tb[TCA_FQ_PIE_ECN_PROB])
	339	q->ecn_prob = nla_get_u32(tb[TCA_FQ_PIE_ECN_PROB]);
	340
	341	if (tb[TCA_FQ_PIE_ECN])
	342	q->p_params.ecn = nla_get_u32(tb[TCA_FQ_PIE_ECN]);
	343
	344	if (tb[TCA_FQ_PIE_BYTEMODE])
	345	q->p_params.bytemode = nla_get_u32(tb[TCA_FQ_PIE_BYTEMODE]);
	346
	347	if (tb[TCA_FQ_PIE_DQ_RATE_ESTIMATOR])
	348	q->p_params.dq_rate_estimator =
	349	nla_get_u32(tb[TCA_FQ_PIE_DQ_RATE_ESTIMATOR]);
	350
	351	/* Drop excess packets if new limit is lower */
	352	while (sch->q.qlen > sch->limit) {
	353	struct sk_buff *skb = fq_pie_qdisc_dequeue(sch);
	354
ec97ecf1 MT	355	len_dropped += qdisc_pkt_len(skb);
ec97ecf1 MT	356	num_dropped += 1;
7a02ea65	357	rtnl_kfree_skbs(skb, skb);
ec97ecf1 MT	358	}
	359	qdisc_tree_reduce_backlog(sch, num_dropped, len_dropped);
	360
	361	sch_tree_unlock(sch);
	362	return 0;
	363
	364	flow_error:
	365	sch_tree_unlock(sch);
	366	return -EINVAL;
	367	}
	368
	369	static void fq_pie_timer(struct timer_list *t)
	370	{
	371	struct fq_pie_sched_data *q = from_timer(q, t, adapt_timer);
	372	struct Qdisc *sch = q->sch;
	373	spinlock_t root_lock; / to lock qdisc for probability calculations */
3a62fed2	374	u32 idx;
ec97ecf1 MT	375
	376	root_lock = qdisc_lock(qdisc_root_sleeping(sch));
	377	spin_lock(root_lock);
	378
	379	for (idx = 0; idx < q->flows_cnt; idx++)
	380	pie_calculate_probability(&q->p_params, &q->flows[idx].vars,
	381	q->flows[idx].backlog);
	382
	383	/* reset the timer to fire after 'tupdate' jiffies. */
	384	if (q->p_params.tupdate)
	385	mod_timer(&q->adapt_timer, jiffies + q->p_params.tupdate);
	386
	387	spin_unlock(root_lock);
	388	}
	389
	390	static int fq_pie_init(struct Qdisc sch, struct nlattr opt,
	391	struct netlink_ext_ack *extack)
	392	{
	393	struct fq_pie_sched_data *q = qdisc_priv(sch);
	394	int err;
3a62fed2	395	u32 idx;
ec97ecf1 MT	396
	397	pie_params_init(&q->p_params);
	398	sch->limit = 10 * 1024;
	399	q->p_params.limit = sch->limit;
	400	q->quantum = psched_mtu(qdisc_dev(sch));
	401	q->sch = sch;
	402	q->ecn_prob = 10;
	403	q->flows_cnt = 1024;
	404	q->memory_limit = SZ_32M;
	405
	406	INIT_LIST_HEAD(&q->new_flows);
	407	INIT_LIST_HEAD(&q->old_flows);
4eef8b1f	408	timer_setup(&q->adapt_timer, fq_pie_timer, 0);
ec97ecf1 MT	409
	410	if (opt) {
	411	err = fq_pie_change(sch, opt, extack);
	412
	413	if (err)
	414	return err;
	415	}
	416
	417	err = tcf_block_get(&q->block, &q->filter_list, sch, extack);
	418	if (err)
	419	goto init_failure;
	420
	421	q->flows = kvcalloc(q->flows_cnt, sizeof(struct fq_pie_flow),
	422	GFP_KERNEL);
	423	if (!q->flows) {
	424	err = -ENOMEM;
	425	goto init_failure;
	426	}
	427	for (idx = 0; idx < q->flows_cnt; idx++) {
	428	struct fq_pie_flow *flow = q->flows + idx;
	429
	430	INIT_LIST_HEAD(&flow->flowchain);
	431	pie_vars_init(&flow->vars);
	432	}
	433
ec97ecf1 MT	434	mod_timer(&q->adapt_timer, jiffies + HZ / 2);
	435
	436	return 0;
	437
	438	init_failure:
	439	q->flows_cnt = 0;
	440
	441	return err;
	442	}
	443
	444	static int fq_pie_dump(struct Qdisc sch, struct sk_buff skb)
	445	{
	446	struct fq_pie_sched_data *q = qdisc_priv(sch);
	447	struct nlattr *opts;
	448
	449	opts = nla_nest_start(skb, TCA_OPTIONS);
	450	if (!opts)
	451	return -EMSGSIZE;
	452
	453	/* convert target from pschedtime to us */
	454	if (nla_put_u32(skb, TCA_FQ_PIE_LIMIT, sch->limit) \|\|
	455	nla_put_u32(skb, TCA_FQ_PIE_FLOWS, q->flows_cnt) \|\|
	456	nla_put_u32(skb, TCA_FQ_PIE_TARGET,
	457	((u32)PSCHED_TICKS2NS(q->p_params.target)) /
	458	NSEC_PER_USEC) \|\|
	459	nla_put_u32(skb, TCA_FQ_PIE_TUPDATE,
	460	jiffies_to_usecs(q->p_params.tupdate)) \|\|
	461	nla_put_u32(skb, TCA_FQ_PIE_ALPHA, q->p_params.alpha) \|\|
	462	nla_put_u32(skb, TCA_FQ_PIE_BETA, q->p_params.beta) \|\|
	463	nla_put_u32(skb, TCA_FQ_PIE_QUANTUM, q->quantum) \|\|
	464	nla_put_u32(skb, TCA_FQ_PIE_MEMORY_LIMIT, q->memory_limit) \|\|
	465	nla_put_u32(skb, TCA_FQ_PIE_ECN_PROB, q->ecn_prob) \|\|
	466	nla_put_u32(skb, TCA_FQ_PIE_ECN, q->p_params.ecn) \|\|
	467	nla_put_u32(skb, TCA_FQ_PIE_BYTEMODE, q->p_params.bytemode) \|\|
	468	nla_put_u32(skb, TCA_FQ_PIE_DQ_RATE_ESTIMATOR,
	469	q->p_params.dq_rate_estimator))
	470	goto nla_put_failure;
	471
	472	return nla_nest_end(skb, opts);
	473
	474	nla_put_failure:
	475	nla_nest_cancel(skb, opts);
	476	return -EMSGSIZE;
	477	}
	478
	479	static int fq_pie_dump_stats(struct Qdisc sch, struct gnet_dump d)
	480	{
	481	struct fq_pie_sched_data *q = qdisc_priv(sch);
	482	struct tc_fq_pie_xstats st = {
	483	.packets_in = q->stats.packets_in,
	484	.overlimit = q->stats.overlimit,
	485	.overmemory = q->overmemory,
	486	.dropped = q->stats.dropped,
	487	.ecn_mark = q->stats.ecn_mark,
	488	.new_flow_count = q->new_flow_count,
	489	.memory_usage = q->memory_usage,
	490	};
	491	struct list_head *pos;
	492
	493	sch_tree_lock(sch);
	494	list_for_each(pos, &q->new_flows)
	495	st.new_flows_len++;
	496
	497	list_for_each(pos, &q->old_flows)
498	st.old_flows_len++;
499	sch_tree_unlock(sch);
500
501	return gnet_stats_copy_app(d, &st, sizeof(st));
502	}
503
504	static void fq_pie_reset(struct Qdisc *sch)
505	{
506	struct fq_pie_sched_data *q = qdisc_priv(sch);
3a62fed2	507	u32 idx;
ec97ecf1 MT	508
	509	INIT_LIST_HEAD(&q->new_flows);
	510	INIT_LIST_HEAD(&q->old_flows);
	511	for (idx = 0; idx < q->flows_cnt; idx++) {
	512	struct fq_pie_flow *flow = q->flows + idx;
	513
	514	/* Removes all packets from flow */
	515	rtnl_kfree_skbs(flow->head, flow->tail);
	516	flow->head = NULL;
	517
	518	INIT_LIST_HEAD(&flow->flowchain);
	519	pie_vars_init(&flow->vars);
	520	}
ec97ecf1 MT	521	}
	522
	523	static void fq_pie_destroy(struct Qdisc *sch)
	524	{
	525	struct fq_pie_sched_data *q = qdisc_priv(sch);
	526
	527	tcf_block_put(q->block);
61c24026	528	q->p_params.tupdate = 0;
ec97ecf1 MT	529	del_timer_sync(&q->adapt_timer);
	530	kvfree(q->flows);
	531	}
	532
	533	static struct Qdisc_ops fq_pie_qdisc_ops __read_mostly = {
	534	.id = "fq_pie",
	535	.priv_size = sizeof(struct fq_pie_sched_data),
	536	.enqueue = fq_pie_qdisc_enqueue,
	537	.dequeue = fq_pie_qdisc_dequeue,
	538	.peek = qdisc_peek_dequeued,
	539	.init = fq_pie_init,
	540	.destroy = fq_pie_destroy,
	541	.reset = fq_pie_reset,
	542	.change = fq_pie_change,
	543	.dump = fq_pie_dump,
	544	.dump_stats = fq_pie_dump_stats,
	545	.owner = THIS_MODULE,
	546	};
	547
	548	static int __init fq_pie_module_init(void)
	549	{
	550	return register_qdisc(&fq_pie_qdisc_ops);
	551	}
	552
	553	static void __exit fq_pie_module_exit(void)
	554	{
	555	unregister_qdisc(&fq_pie_qdisc_ops);
	556	}
	557
	558	module_init(fq_pie_module_init);
	559	module_exit(fq_pie_module_exit);
	560
	561	MODULE_DESCRIPTION("Flow Queue Proportional Integral controller Enhanced (FQ-PIE)");
	562	MODULE_AUTHOR("Mohit P. Tahiliani");
	563	MODULE_LICENSE("GPL");