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

// SPDX-License-Identifier: GPL-2.0-only
/*
 * net/sched/sch_choke.c	CHOKE scheduler
 *
 * Copyright (c) 2011 Stephen Hemminger <shemminger@vyatta.com>
 * Copyright (c) 2011 Eric Dumazet <eric.dumazet@gmail.com>
 */

#include <linux/module.h>
#include <linux/types.h>
#include <linux/kernel.h>
#include <linux/skbuff.h>
#include <linux/vmalloc.h>
#include <net/pkt_sched.h>
#include <net/pkt_cls.h>
#include <net/inet_ecn.h>
#include <net/red.h>
#include <net/flow_dissector.h>

/*
   CHOKe stateless AQM for fair bandwidth allocation
   =================================================

   CHOKe (CHOose and Keep for responsive flows, CHOose and Kill for
   unresponsive flows) is a variant of RED that penalizes misbehaving flows but
   maintains no flow state. The difference from RED is an additional step
   during the enqueuing process. If average queue size is over the
   low threshold (qmin), a packet is chosen at random from the queue.
   If both the new and chosen packet are from the same flow, both
   are dropped. Unlike RED, CHOKe is not really a "classful" qdisc because it
   needs to access packets in queue randomly. It has a minimal class
   interface to allow overriding the builtin flow classifier with
   filters.

   Source:
   R. Pan, B. Prabhakar, and K. Psounis, "CHOKe, A Stateless
   Active Queue Management Scheme for Approximating Fair Bandwidth Allocation",
   IEEE INFOCOM, 2000.

   A. Tang, J. Wang, S. Low, "Understanding CHOKe: Throughput and Spatial
   Characteristics", IEEE/ACM Transactions on Networking, 2004

 */

/* Upper bound on size of sk_buff table (packets) */
#define CHOKE_MAX_QUEUE	(128*1024 - 1)

struct choke_sched_data {
/* Parameters */
	u32		 limit;
	unsigned char	 flags;

	struct red_parms parms;

/* Variables */
	struct red_vars  vars;
	struct {
		u32	prob_drop;	/* Early probability drops */
		u32	prob_mark;	/* Early probability marks */
		u32	forced_drop;	/* Forced drops, qavg > max_thresh */
		u32	forced_mark;	/* Forced marks, qavg > max_thresh */
		u32	pdrop;          /* Drops due to queue limits */
		u32	matched;	/* Drops to flow match */
	} stats;

	unsigned int	 head;
	unsigned int	 tail;

	unsigned int	 tab_mask; /* size - 1 */

	struct sk_buff **tab;
};

/* number of elements in queue including holes */
static unsigned int choke_len(const struct choke_sched_data *q)
{
	return (q->tail - q->head) & q->tab_mask;
}

/* Is ECN parameter configured */
static int use_ecn(const struct choke_sched_data *q)
{
	return q->flags & TC_RED_ECN;
}

/* Should packets over max just be dropped (versus marked) */
static int use_harddrop(const struct choke_sched_data *q)
{
	return q->flags & TC_RED_HARDDROP;
}

/* Move head pointer forward to skip over holes */
static void choke_zap_head_holes(struct choke_sched_data *q)
{
	do {
		q->head = (q->head + 1) & q->tab_mask;
		if (q->head == q->tail)
			break;
	} while (q->tab[q->head] == NULL);
}

/* Move tail pointer backwards to reuse holes */
static void choke_zap_tail_holes(struct choke_sched_data *q)
{
	do {
		q->tail = (q->tail - 1) & q->tab_mask;
		if (q->head == q->tail)
			break;
	} while (q->tab[q->tail] == NULL);
}

/* Drop packet from queue array by creating a "hole" */
static void choke_drop_by_idx(struct Qdisc *sch, unsigned int idx,
			      struct sk_buff **to_free)
{
	struct choke_sched_data *q = qdisc_priv(sch);
	struct sk_buff *skb = q->tab[idx];

	q->tab[idx] = NULL;

	if (idx == q->head)
		choke_zap_head_holes(q);
	if (idx == q->tail)
		choke_zap_tail_holes(q);

	qdisc_qstats_backlog_dec(sch, skb);
	qdisc_tree_reduce_backlog(sch, 1, qdisc_pkt_len(skb));
	qdisc_drop(skb, sch, to_free);
	--sch->q.qlen;
}

struct choke_skb_cb {
	u8			keys_valid;
	struct			flow_keys_digest keys;
};

static inline struct choke_skb_cb *choke_skb_cb(const struct sk_buff *skb)
{
	qdisc_cb_private_validate(skb, sizeof(struct choke_skb_cb));
	return (struct choke_skb_cb *)qdisc_skb_cb(skb)->data;
}

/*
 * Compare flow of two packets
 *  Returns true only if source and destination address and port match.
 *          false for special cases
 */
static bool choke_match_flow(struct sk_buff *skb1,
			     struct sk_buff *skb2)
{
	struct flow_keys temp;

	if (skb1->protocol != skb2->protocol)
		return false;

	if (!choke_skb_cb(skb1)->keys_valid) {
		choke_skb_cb(skb1)->keys_valid = 1;
		skb_flow_dissect_flow_keys(skb1, &temp, 0);
		make_flow_keys_digest(&choke_skb_cb(skb1)->keys, &temp);
	}

	if (!choke_skb_cb(skb2)->keys_valid) {
		choke_skb_cb(skb2)->keys_valid = 1;
		skb_flow_dissect_flow_keys(skb2, &temp, 0);
		make_flow_keys_digest(&choke_skb_cb(skb2)->keys, &temp);
	}

	return !memcmp(&choke_skb_cb(skb1)->keys,
		       &choke_skb_cb(skb2)->keys,
		       sizeof(choke_skb_cb(skb1)->keys));
}

/*
 * Select a packet at random from queue
 * HACK: since queue can have holes from previous deletion; retry several
 *   times to find a random skb but then just give up and return the head
 * Will return NULL if queue is empty (q->head == q->tail)
 */
static struct sk_buff *choke_peek_random(const struct choke_sched_data *q,
					 unsigned int *pidx)
{
	struct sk_buff *skb;
	int retrys = 3;

	do {
		*pidx = (q->head + get_random_u32_below(choke_len(q))) & q->tab_mask;
		skb = q->tab[*pidx];
		if (skb)
			return skb;
	} while (--retrys > 0);

	return q->tab[*pidx = q->head];
}

/*
 * Compare new packet with random packet in queue
 * returns true if matched and sets *pidx
 */
static bool choke_match_random(const struct choke_sched_data *q,
			       struct sk_buff *nskb,
			       unsigned int *pidx)
{
	struct sk_buff *oskb;

	if (q->head == q->tail)
		return false;

	oskb = choke_peek_random(q, pidx);
	return choke_match_flow(oskb, nskb);
}

static int choke_enqueue(struct sk_buff *skb, struct Qdisc *sch,
			 struct sk_buff **to_free)
{
	struct choke_sched_data *q = qdisc_priv(sch);
	const struct red_parms *p = &q->parms;

	choke_skb_cb(skb)->keys_valid = 0;
	/* Compute average queue usage (see RED) */
	q->vars.qavg = red_calc_qavg(p, &q->vars, sch->q.qlen);
	if (red_is_idling(&q->vars))
		red_end_of_idle_period(&q->vars);

	/* Is queue small? */
	if (q->vars.qavg <= p->qth_min)
		q->vars.qcount = -1;
	else {
		unsigned int idx;

		/* Draw a packet at random from queue and compare flow */
		if (choke_match_random(q, skb, &idx)) {
			q->stats.matched++;
			choke_drop_by_idx(sch, idx, to_free);
			goto congestion_drop;
		}

		/* Queue is large, always mark/drop */
		if (q->vars.qavg > p->qth_max) {
			q->vars.qcount = -1;

			qdisc_qstats_overlimit(sch);
			if (use_harddrop(q) || !use_ecn(q) ||
			    !INET_ECN_set_ce(skb)) {
				q->stats.forced_drop++;
				goto congestion_drop;
			}

			q->stats.forced_mark++;
		} else if (++q->vars.qcount) {
			if (red_mark_probability(p, &q->vars, q->vars.qavg)) {
				q->vars.qcount = 0;
				q->vars.qR = red_random(p);

				qdisc_qstats_overlimit(sch);
				if (!use_ecn(q) || !INET_ECN_set_ce(skb)) {
					q->stats.prob_drop++;
					goto congestion_drop;
				}

				q->stats.prob_mark++;
			}
		} else
			q->vars.qR = red_random(p);
	}

	/* Admit new packet */
	if (sch->q.qlen < q->limit) {
		q->tab[q->tail] = skb;
		q->tail = (q->tail + 1) & q->tab_mask;
		++sch->q.qlen;
		qdisc_qstats_backlog_inc(sch, skb);
		return NET_XMIT_SUCCESS;
	}

	q->stats.pdrop++;
	return qdisc_drop(skb, sch, to_free);

congestion_drop:
	qdisc_drop(skb, sch, to_free);
	return NET_XMIT_CN;
}

static struct sk_buff *choke_dequeue(struct Qdisc *sch)
{
	struct choke_sched_data *q = qdisc_priv(sch);
	struct sk_buff *skb;

	if (q->head == q->tail) {
		if (!red_is_idling(&q->vars))
			red_start_of_idle_period(&q->vars);
		return NULL;
	}

	skb = q->tab[q->head];
	q->tab[q->head] = NULL;
	choke_zap_head_holes(q);
	--sch->q.qlen;
	qdisc_qstats_backlog_dec(sch, skb);
	qdisc_bstats_update(sch, skb);

	return skb;
}

static void choke_reset(struct Qdisc *sch)
{
	struct choke_sched_data *q = qdisc_priv(sch);

	while (q->head != q->tail) {
		struct sk_buff *skb = q->tab[q->head];

		q->head = (q->head + 1) & q->tab_mask;
		if (!skb)
			continue;
		rtnl_qdisc_drop(skb, sch);
	}

	if (q->tab)
		memset(q->tab, 0, (q->tab_mask + 1) * sizeof(struct sk_buff *));
	q->head = q->tail = 0;
	red_restart(&q->vars);
}

static const struct nla_policy choke_policy[TCA_CHOKE_MAX + 1] = {
	[TCA_CHOKE_PARMS]	= { .len = sizeof(struct tc_red_qopt) },
	[TCA_CHOKE_STAB]	= { .len = RED_STAB_SIZE },
	[TCA_CHOKE_MAX_P]	= { .type = NLA_U32 },
};


static void choke_free(void *addr)
{
	kvfree(addr);
}

static int choke_change(struct Qdisc *sch, struct nlattr *opt,
			struct netlink_ext_ack *extack)
{
	struct choke_sched_data *q = qdisc_priv(sch);
	struct nlattr *tb[TCA_CHOKE_MAX + 1];
	const struct tc_red_qopt *ctl;
	int err;
	struct sk_buff **old = NULL;
	unsigned int mask;
	u32 max_P;
	u8 *stab;

	if (opt == NULL)
		return -EINVAL;

	err = nla_parse_nested_deprecated(tb, TCA_CHOKE_MAX, opt,
					  choke_policy, NULL);
	if (err < 0)
		return err;

	if (tb[TCA_CHOKE_PARMS] == NULL ||
	    tb[TCA_CHOKE_STAB] == NULL)
		return -EINVAL;

	max_P = tb[TCA_CHOKE_MAX_P] ? nla_get_u32(tb[TCA_CHOKE_MAX_P]) : 0;

	ctl = nla_data(tb[TCA_CHOKE_PARMS]);
	stab = nla_data(tb[TCA_CHOKE_STAB]);
	if (!red_check_params(ctl->qth_min, ctl->qth_max, ctl->Wlog, ctl->Scell_log, stab))
		return -EINVAL;

	if (ctl->limit > CHOKE_MAX_QUEUE)
		return -EINVAL;

	mask = roundup_pow_of_two(ctl->limit + 1) - 1;
	if (mask != q->tab_mask) {
		struct sk_buff **ntab;

		ntab = kvcalloc(mask + 1, sizeof(struct sk_buff *), GFP_KERNEL);
		if (!ntab)
			return -ENOMEM;

		sch_tree_lock(sch);
		old = q->tab;
		if (old) {
			unsigned int oqlen = sch->q.qlen, tail = 0;
			unsigned dropped = 0;

			while (q->head != q->tail) {
				struct sk_buff *skb = q->tab[q->head];

				q->head = (q->head + 1) & q->tab_mask;
				if (!skb)
					continue;
				if (tail < mask) {
					ntab[tail++] = skb;
					continue;
				}
				dropped += qdisc_pkt_len(skb);
				qdisc_qstats_backlog_dec(sch, skb);
				--sch->q.qlen;
				rtnl_qdisc_drop(skb, sch);
			}
			qdisc_tree_reduce_backlog(sch, oqlen - sch->q.qlen, dropped);
			q->head = 0;
			q->tail = tail;
		}

		q->tab_mask = mask;
		q->tab = ntab;
	} else
		sch_tree_lock(sch);

	q->flags = ctl->flags;
	q->limit = ctl->limit;

	red_set_parms(&q->parms, ctl->qth_min, ctl->qth_max, ctl->Wlog,
		      ctl->Plog, ctl->Scell_log,
		      stab,
		      max_P);
	red_set_vars(&q->vars);

	if (q->head == q->tail)
		red_end_of_idle_period(&q->vars);

	sch_tree_unlock(sch);
	choke_free(old);
	return 0;
}

static int choke_init(struct Qdisc *sch, struct nlattr *opt,
		      struct netlink_ext_ack *extack)
{
	return choke_change(sch, opt, extack);
}

static int choke_dump(struct Qdisc *sch, struct sk_buff *skb)
{
	struct choke_sched_data *q = qdisc_priv(sch);
	struct nlattr *opts = NULL;
	struct tc_red_qopt opt = {
		.limit		= q->limit,
		.flags		= q->flags,
		.qth_min	= q->parms.qth_min >> q->parms.Wlog,
		.qth_max	= q->parms.qth_max >> q->parms.Wlog,
		.Wlog		= q->parms.Wlog,
		.Plog		= q->parms.Plog,
		.Scell_log	= q->parms.Scell_log,
	};

	opts = nla_nest_start_noflag(skb, TCA_OPTIONS);
	if (opts == NULL)
		goto nla_put_failure;

	if (nla_put(skb, TCA_CHOKE_PARMS, sizeof(opt), &opt) ||
	    nla_put_u32(skb, TCA_CHOKE_MAX_P, q->parms.max_P))
		goto nla_put_failure;
	return nla_nest_end(skb, opts);

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

static int choke_dump_stats(struct Qdisc *sch, struct gnet_dump *d)
{
	struct choke_sched_data *q = qdisc_priv(sch);
	struct tc_choke_xstats st = {
		.early	= q->stats.prob_drop + q->stats.forced_drop,
		.marked	= q->stats.prob_mark + q->stats.forced_mark,
		.pdrop	= q->stats.pdrop,
		.matched = q->stats.matched,
	};

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

static void choke_destroy(struct Qdisc *sch)
{
	struct choke_sched_data *q = qdisc_priv(sch);

	choke_free(q->tab);
}

static struct sk_buff *choke_peek_head(struct Qdisc *sch)
{
	struct choke_sched_data *q = qdisc_priv(sch);

	return (q->head != q->tail) ? q->tab[q->head] : NULL;
}

static struct Qdisc_ops choke_qdisc_ops __read_mostly = {
	.id		=	"choke",
	.priv_size	=	sizeof(struct choke_sched_data),

	.enqueue	=	choke_enqueue,
	.dequeue	=	choke_dequeue,
	.peek		=	choke_peek_head,
	.init		=	choke_init,
	.destroy	=	choke_destroy,
	.reset		=	choke_reset,
	.change		=	choke_change,
	.dump		=	choke_dump,
	.dump_stats	=	choke_dump_stats,
	.owner		=	THIS_MODULE,
};

static int __init choke_module_init(void)
{
	return register_qdisc(&choke_qdisc_ops);
}

static void __exit choke_module_exit(void)
{
	unregister_qdisc(&choke_qdisc_ops);
}

module_init(choke_module_init)
module_exit(choke_module_exit)

MODULE_LICENSE("GPL");
Commit	Line	Data
d2912cb1	1	// SPDX-License-Identifier: GPL-2.0-only
45e14433	2	/*
	3	* net/sched/sch_choke.c CHOKE scheduler
	4	*
	5	* Copyright (c) 2011 Stephen Hemminger <shemminger@vyatta.com>
	6	* Copyright (c) 2011 Eric Dumazet <eric.dumazet@gmail.com>
45e14433	7	*/
	8
	9	#include <linux/module.h>
	10	#include <linux/types.h>
	11	#include <linux/kernel.h>
	12	#include <linux/skbuff.h>
cdfb74d4	13	#include <linux/vmalloc.h>
45e14433	14	#include <net/pkt_sched.h>
cf1facda	15	#include <net/pkt_cls.h>
45e14433	16	#include <net/inet_ecn.h>
45e14433	17	#include <net/red.h>
1bd758eb	18	#include <net/flow_dissector.h>
45e14433	19
	20	/*
	21	CHOKe stateless AQM for fair bandwidth allocation
	22	=================================================
	23
	24	CHOKe (CHOose and Keep for responsive flows, CHOose and Kill for
	25	unresponsive flows) is a variant of RED that penalizes misbehaving flows but
	26	maintains no flow state. The difference from RED is an additional step
	27	during the enqueuing process. If average queue size is over the
	28	low threshold (qmin), a packet is chosen at random from the queue.
	29	If both the new and chosen packet are from the same flow, both
	30	are dropped. Unlike RED, CHOKe is not really a "classful" qdisc because it
	31	needs to access packets in queue randomly. It has a minimal class
	32	interface to allow overriding the builtin flow classifier with
	33	filters.
	34
	35	Source:
	36	R. Pan, B. Prabhakar, and K. Psounis, "CHOKe, A Stateless
	37	Active Queue Management Scheme for Approximating Fair Bandwidth Allocation",
	38	IEEE INFOCOM, 2000.
	39
	40	A. Tang, J. Wang, S. Low, "Understanding CHOKe: Throughput and Spatial
	41	Characteristics", IEEE/ACM Transactions on Networking, 2004
	42
	43	*/
	44
	45	/* Upper bound on size of sk_buff table (packets) */
	46	#define CHOKE_MAX_QUEUE (128*1024 - 1)
	47
	48	struct choke_sched_data {
	49	/* Parameters */
	50	u32 limit;
	51	unsigned char flags;
	52
	53	struct red_parms parms;
	54
	55	/* Variables */
eeca6688	56	struct red_vars vars;
45e14433	57	struct {
	58	u32 prob_drop; /* Early probability drops */
	59	u32 prob_mark; /* Early probability marks */
	60	u32 forced_drop; /* Forced drops, qavg > max_thresh */
	61	u32 forced_mark; /* Forced marks, qavg > max_thresh */
	62	u32 pdrop; /* Drops due to queue limits */
45e14433	63	u32 matched; /* Drops to flow match */
	64	} stats;
	65
	66	unsigned int head;
	67	unsigned int tail;
	68
	69	unsigned int tab_mask; /* size - 1 */
	70
	71	struct sk_buff **tab;
	72	};
	73
45e14433	74	/* number of elements in queue including holes */
	75	static unsigned int choke_len(const struct choke_sched_data *q)
	76	{
	77	return (q->tail - q->head) & q->tab_mask;
	78	}
	79
	80	/* Is ECN parameter configured */
	81	static int use_ecn(const struct choke_sched_data *q)
	82	{
	83	return q->flags & TC_RED_ECN;
	84	}
	85
	86	/* Should packets over max just be dropped (versus marked) */
	87	static int use_harddrop(const struct choke_sched_data *q)
	88	{
	89	return q->flags & TC_RED_HARDDROP;
	90	}
	91
	92	/* Move head pointer forward to skip over holes */
	93	static void choke_zap_head_holes(struct choke_sched_data *q)
	94	{
	95	do {
	96	q->head = (q->head + 1) & q->tab_mask;
	97	if (q->head == q->tail)
	98	break;
	99	} while (q->tab[q->head] == NULL);
	100	}
	101
	102	/* Move tail pointer backwards to reuse holes */
	103	static void choke_zap_tail_holes(struct choke_sched_data *q)
	104	{
	105	do {
	106	q->tail = (q->tail - 1) & q->tab_mask;
	107	if (q->head == q->tail)
	108	break;
	109	} while (q->tab[q->tail] == NULL);
	110	}
	111
	112	/* Drop packet from queue array by creating a "hole" */
520ac30f ED	113	static void choke_drop_by_idx(struct Qdisc *sch, unsigned int idx,
520ac30f ED	114	struct sk_buff **to_free)
45e14433	115	{
	116	struct choke_sched_data *q = qdisc_priv(sch);
	117	struct sk_buff *skb = q->tab[idx];
	118
	119	q->tab[idx] = NULL;
	120
	121	if (idx == q->head)
	122	choke_zap_head_holes(q);
	123	if (idx == q->tail)
	124	choke_zap_tail_holes(q);
	125
25331d6c	126	qdisc_qstats_backlog_dec(sch, skb);
2ccccf5f	127	qdisc_tree_reduce_backlog(sch, 1, qdisc_pkt_len(skb));
520ac30f	128	qdisc_drop(skb, sch, to_free);
45e14433	129	--sch->q.qlen;
	130	}
	131
26f70e12	132	struct choke_skb_cb {
2bcc34bb	133	u8 keys_valid;
2e99403d	134	struct flow_keys_digest keys;
26f70e12 ED	135	};
	136
	137	static inline struct choke_skb_cb choke_skb_cb(const struct sk_buff skb)
	138	{
16bda13d	139	qdisc_cb_private_validate(skb, sizeof(struct choke_skb_cb));
26f70e12 ED	140	return (struct choke_skb_cb *)qdisc_skb_cb(skb)->data;
	141	}
	142
2bcc34bb ED	143	/*
	144	* Compare flow of two packets
	145	* Returns true only if source and destination address and port match.
	146	* false for special cases
	147	*/
	148	static bool choke_match_flow(struct sk_buff *skb1,
	149	struct sk_buff *skb2)
	150	{
25711786 ED	151	struct flow_keys temp;
25711786 ED	152
2bcc34bb ED	153	if (skb1->protocol != skb2->protocol)
	154	return false;
	155
	156	if (!choke_skb_cb(skb1)->keys_valid) {
	157	choke_skb_cb(skb1)->keys_valid = 1;
cd79a238	158	skb_flow_dissect_flow_keys(skb1, &temp, 0);
2e99403d	159	make_flow_keys_digest(&choke_skb_cb(skb1)->keys, &temp);
2bcc34bb ED	160	}
	161
	162	if (!choke_skb_cb(skb2)->keys_valid) {
	163	choke_skb_cb(skb2)->keys_valid = 1;
cd79a238	164	skb_flow_dissect_flow_keys(skb2, &temp, 0);
2e99403d	165	make_flow_keys_digest(&choke_skb_cb(skb2)->keys, &temp);
2bcc34bb ED	166	}
	167
	168	return !memcmp(&choke_skb_cb(skb1)->keys,
	169	&choke_skb_cb(skb2)->keys,
2e99403d	170	sizeof(choke_skb_cb(skb1)->keys));
2bcc34bb ED	171	}
2bcc34bb ED	172
45e14433	173	/*
	174	* Select a packet at random from queue
	175	* HACK: since queue can have holes from previous deletion; retry several
	176	* times to find a random skb but then just give up and return the head
	177	* Will return NULL if queue is empty (q->head == q->tail)
	178	*/
	179	static struct sk_buff choke_peek_random(const struct choke_sched_data q,
	180	unsigned int *pidx)
	181	{
	182	struct sk_buff *skb;
	183	int retrys = 3;
	184
	185	do {
8032bf12	186	*pidx = (q->head + get_random_u32_below(choke_len(q))) & q->tab_mask;
45e14433	187	skb = q->tab[*pidx];
	188	if (skb)
	189	return skb;
	190	} while (--retrys > 0);
	191
	192	return q->tab[*pidx = q->head];
	193	}
	194
	195	/*
	196	* Compare new packet with random packet in queue
	197	* returns true if matched and sets *pidx
	198	*/
	199	static bool choke_match_random(const struct choke_sched_data *q,
	200	struct sk_buff *nskb,
	201	unsigned int *pidx)
	202	{
	203	struct sk_buff *oskb;
	204
	205	if (q->head == q->tail)
	206	return false;
	207
	208	oskb = choke_peek_random(q, pidx);
45e14433	209	return choke_match_flow(oskb, nskb);
	210	}
	211
520ac30f ED	212	static int choke_enqueue(struct sk_buff skb, struct Qdisc sch,
520ac30f ED	213	struct sk_buff **to_free)
45e14433	214	{
45e14433	215	struct choke_sched_data *q = qdisc_priv(sch);
eeca6688	216	const struct red_parms *p = &q->parms;
45e14433	217
2bcc34bb	218	choke_skb_cb(skb)->keys_valid = 0;
45e14433	219	/* Compute average queue usage (see RED) */
eeca6688 ED	220	q->vars.qavg = red_calc_qavg(p, &q->vars, sch->q.qlen);
	221	if (red_is_idling(&q->vars))
	222	red_end_of_idle_period(&q->vars);
45e14433	223
45e14433	224	/* Is queue small? */
eeca6688 ED	225	if (q->vars.qavg <= p->qth_min)
eeca6688 ED	226	q->vars.qcount = -1;
45e14433	227	else {
	228	unsigned int idx;
	229
	230	/* Draw a packet at random from queue and compare flow */
	231	if (choke_match_random(q, skb, &idx)) {
	232	q->stats.matched++;
520ac30f	233	choke_drop_by_idx(sch, idx, to_free);
45e14433	234	goto congestion_drop;
	235	}
	236
	237	/* Queue is large, always mark/drop */
eeca6688 ED	238	if (q->vars.qavg > p->qth_max) {
eeca6688 ED	239	q->vars.qcount = -1;
45e14433	240
25331d6c	241	qdisc_qstats_overlimit(sch);
45e14433	242	if (use_harddrop(q) \|\| !use_ecn(q) \|\|
	243	!INET_ECN_set_ce(skb)) {
	244	q->stats.forced_drop++;
	245	goto congestion_drop;
	246	}
	247
	248	q->stats.forced_mark++;
eeca6688 ED	249	} else if (++q->vars.qcount) {
	250	if (red_mark_probability(p, &q->vars, q->vars.qavg)) {
	251	q->vars.qcount = 0;
	252	q->vars.qR = red_random(p);
45e14433	253
25331d6c	254	qdisc_qstats_overlimit(sch);
45e14433	255	if (!use_ecn(q) \|\| !INET_ECN_set_ce(skb)) {
	256	q->stats.prob_drop++;
	257	goto congestion_drop;
	258	}
	259
	260	q->stats.prob_mark++;
	261	}
	262	} else
eeca6688	263	q->vars.qR = red_random(p);
45e14433	264	}
	265
	266	/* Admit new packet */
	267	if (sch->q.qlen < q->limit) {
	268	q->tab[q->tail] = skb;
	269	q->tail = (q->tail + 1) & q->tab_mask;
	270	++sch->q.qlen;
25331d6c	271	qdisc_qstats_backlog_inc(sch, skb);
45e14433	272	return NET_XMIT_SUCCESS;
	273	}
	274
	275	q->stats.pdrop++;
520ac30f	276	return qdisc_drop(skb, sch, to_free);
45e14433	277
17045755	278	congestion_drop:
520ac30f	279	qdisc_drop(skb, sch, to_free);
45e14433	280	return NET_XMIT_CN;
45e14433	281	}
	282
	283	static struct sk_buff choke_dequeue(struct Qdisc sch)
	284	{
	285	struct choke_sched_data *q = qdisc_priv(sch);
	286	struct sk_buff *skb;
	287
	288	if (q->head == q->tail) {
eeca6688 ED	289	if (!red_is_idling(&q->vars))
eeca6688 ED	290	red_start_of_idle_period(&q->vars);
45e14433	291	return NULL;
	292	}
	293
	294	skb = q->tab[q->head];
	295	q->tab[q->head] = NULL;
	296	choke_zap_head_holes(q);
	297	--sch->q.qlen;
25331d6c	298	qdisc_qstats_backlog_dec(sch, skb);
45e14433	299	qdisc_bstats_update(sch, skb);
	300
	301	return skb;
	302	}
	303
45e14433	304	static void choke_reset(struct Qdisc *sch)
	305	{
	306	struct choke_sched_data *q = qdisc_priv(sch);
	307
77e62da6 WC	308	while (q->head != q->tail) {
	309	struct sk_buff *skb = q->tab[q->head];
	310
	311	q->head = (q->head + 1) & q->tab_mask;
	312	if (!skb)
	313	continue;
f9aed311	314	rtnl_qdisc_drop(skb, sch);
77e62da6 WC	315	}
77e62da6 WC	316
8738c85c ED	317	if (q->tab)
8738c85c ED	318	memset(q->tab, 0, (q->tab_mask + 1) * sizeof(struct sk_buff *));
77e62da6	319	q->head = q->tail = 0;
eeca6688	320	red_restart(&q->vars);
45e14433	321	}
	322
	323	static const struct nla_policy choke_policy[TCA_CHOKE_MAX + 1] = {
	324	[TCA_CHOKE_PARMS] = { .len = sizeof(struct tc_red_qopt) },
	325	[TCA_CHOKE_STAB] = { .len = RED_STAB_SIZE },
a73ed26b	326	[TCA_CHOKE_MAX_P] = { .type = NLA_U32 },
45e14433	327	};
	328
	329
	330	static void choke_free(void *addr)
	331	{
4cb28970	332	kvfree(addr);
45e14433	333	}
45e14433	334
2030721c AA	335	static int choke_change(struct Qdisc sch, struct nlattr opt,
2030721c AA	336	struct netlink_ext_ack *extack)
45e14433	337	{
	338	struct choke_sched_data *q = qdisc_priv(sch);
	339	struct nlattr *tb[TCA_CHOKE_MAX + 1];
	340	const struct tc_red_qopt *ctl;
	341	int err;
	342	struct sk_buff **old = NULL;
	343	unsigned int mask;
a73ed26b	344	u32 max_P;
e323d865	345	u8 *stab;
45e14433	346
	347	if (opt == NULL)
	348	return -EINVAL;
	349
8cb08174 JB	350	err = nla_parse_nested_deprecated(tb, TCA_CHOKE_MAX, opt,
8cb08174 JB	351	choke_policy, NULL);
45e14433	352	if (err < 0)
	353	return err;
	354
	355	if (tb[TCA_CHOKE_PARMS] == NULL \|\|
	356	tb[TCA_CHOKE_STAB] == NULL)
	357	return -EINVAL;
	358
a73ed26b ED	359	max_P = tb[TCA_CHOKE_MAX_P] ? nla_get_u32(tb[TCA_CHOKE_MAX_P]) : 0;
a73ed26b ED	360
45e14433	361	ctl = nla_data(tb[TCA_CHOKE_PARMS]);
e323d865 ED	362	stab = nla_data(tb[TCA_CHOKE_STAB]);
e323d865 ED	363	if (!red_check_params(ctl->qth_min, ctl->qth_max, ctl->Wlog, ctl->Scell_log, stab))
8afa10cb NF	364	return -EINVAL;
8afa10cb NF	365
45e14433	366	if (ctl->limit > CHOKE_MAX_QUEUE)
	367	return -EINVAL;
	368
	369	mask = roundup_pow_of_two(ctl->limit + 1) - 1;
	370	if (mask != q->tab_mask) {
	371	struct sk_buff **ntab;
	372
793da4bf	373	ntab = kvcalloc(mask + 1, sizeof(struct sk_buff *), GFP_KERNEL);
45e14433	374	if (!ntab)
	375	return -ENOMEM;
	376
	377	sch_tree_lock(sch);
	378	old = q->tab;
	379	if (old) {
	380	unsigned int oqlen = sch->q.qlen, tail = 0;
2ccccf5f	381	unsigned dropped = 0;
45e14433	382
	383	while (q->head != q->tail) {
	384	struct sk_buff *skb = q->tab[q->head];
	385
	386	q->head = (q->head + 1) & q->tab_mask;
	387	if (!skb)
	388	continue;
	389	if (tail < mask) {
	390	ntab[tail++] = skb;
	391	continue;
	392	}
2ccccf5f	393	dropped += qdisc_pkt_len(skb);
25331d6c	394	qdisc_qstats_backlog_dec(sch, skb);
45e14433	395	--sch->q.qlen;
f9aed311	396	rtnl_qdisc_drop(skb, sch);
45e14433	397	}
2ccccf5f	398	qdisc_tree_reduce_backlog(sch, oqlen - sch->q.qlen, dropped);
45e14433	399	q->head = 0;
	400	q->tail = tail;
	401	}
	402
	403	q->tab_mask = mask;
	404	q->tab = ntab;
	405	} else
	406	sch_tree_lock(sch);
	407
	408	q->flags = ctl->flags;
	409	q->limit = ctl->limit;
	410
	411	red_set_parms(&q->parms, ctl->qth_min, ctl->qth_max, ctl->Wlog,
	412	ctl->Plog, ctl->Scell_log,
e323d865	413	stab,
a73ed26b	414	max_P);
eeca6688	415	red_set_vars(&q->vars);
45e14433	416
45e14433	417	if (q->head == q->tail)
eeca6688	418	red_end_of_idle_period(&q->vars);
45e14433	419
	420	sch_tree_unlock(sch);
	421	choke_free(old);
	422	return 0;
	423	}
	424
e63d7dfd AA	425	static int choke_init(struct Qdisc sch, struct nlattr opt,
e63d7dfd AA	426	struct netlink_ext_ack *extack)
45e14433	427	{
2030721c	428	return choke_change(sch, opt, extack);
45e14433	429	}
	430
	431	static int choke_dump(struct Qdisc sch, struct sk_buff skb)
	432	{
	433	struct choke_sched_data *q = qdisc_priv(sch);
	434	struct nlattr *opts = NULL;
	435	struct tc_red_qopt opt = {
	436	.limit = q->limit,
	437	.flags = q->flags,
	438	.qth_min = q->parms.qth_min >> q->parms.Wlog,
	439	.qth_max = q->parms.qth_max >> q->parms.Wlog,
	440	.Wlog = q->parms.Wlog,
	441	.Plog = q->parms.Plog,
	442	.Scell_log = q->parms.Scell_log,
	443	};
	444
ae0be8de	445	opts = nla_nest_start_noflag(skb, TCA_OPTIONS);
45e14433	446	if (opts == NULL)
	447	goto nla_put_failure;
	448
1b34ec43 DM	449	if (nla_put(skb, TCA_CHOKE_PARMS, sizeof(opt), &opt) \|\|
	450	nla_put_u32(skb, TCA_CHOKE_MAX_P, q->parms.max_P))
	451	goto nla_put_failure;
45e14433	452	return nla_nest_end(skb, opts);
	453
	454	nla_put_failure:
	455	nla_nest_cancel(skb, opts);
	456	return -EMSGSIZE;
	457	}
	458
	459	static int choke_dump_stats(struct Qdisc sch, struct gnet_dump d)
	460	{
	461	struct choke_sched_data *q = qdisc_priv(sch);
	462	struct tc_choke_xstats st = {
	463	.early = q->stats.prob_drop + q->stats.forced_drop,
	464	.marked = q->stats.prob_mark + q->stats.forced_mark,
	465	.pdrop = q->stats.pdrop,
45e14433	466	.matched = q->stats.matched,
	467	};
	468
	469	return gnet_stats_copy_app(d, &st, sizeof(st));
	470	}
	471
	472	static void choke_destroy(struct Qdisc *sch)
	473	{
	474	struct choke_sched_data *q = qdisc_priv(sch);
	475
45e14433	476	choke_free(q->tab);
	477	}
	478
45e14433	479	static struct sk_buff choke_peek_head(struct Qdisc sch)
	480	{
	481	struct choke_sched_data *q = qdisc_priv(sch);
	482
	483	return (q->head != q->tail) ? q->tab[q->head] : NULL;
	484	}
	485
	486	static struct Qdisc_ops choke_qdisc_ops __read_mostly = {
	487	.id = "choke",
	488	.priv_size = sizeof(struct choke_sched_data),
	489
	490	.enqueue = choke_enqueue,
	491	.dequeue = choke_dequeue,
	492	.peek = choke_peek_head,
45e14433	493	.init = choke_init,
	494	.destroy = choke_destroy,
	495	.reset = choke_reset,
	496	.change = choke_change,
	497	.dump = choke_dump,
	498	.dump_stats = choke_dump_stats,
	499	.owner = THIS_MODULE,
	500	};
	501
	502	static int __init choke_module_init(void)
	503	{
	504	return register_qdisc(&choke_qdisc_ops);
	505	}
	506
	507	static void __exit choke_module_exit(void)
	508	{
	509	unregister_qdisc(&choke_qdisc_ops);
	510	}
	511
	512	module_init(choke_module_init)
	513	module_exit(choke_module_exit)
	514
	515	MODULE_LICENSE("GPL");