1/* SPDX-License-Identifier: GPL-2.0-only */
  2/*
  3 * Copyright (c) 2016 Qualcomm Atheros, Inc
  4 *
  5 * Based on net/sched/sch_fq_codel.c
  6 */
  7#ifndef __NET_SCHED_FQ_IMPL_H
  8#define __NET_SCHED_FQ_IMPL_H
  9
 10#include <net/fq.h>
 11
 12/* functions that are embedded into includer */
 13
 14
 15static void
 16__fq_adjust_removal(struct fq *fq, struct fq_flow *flow, unsigned int packets,
 17		    unsigned int bytes, unsigned int truesize)
 18{
 19	struct fq_tin *tin = flow->tin;
 20	int idx;
 21
 22	tin->backlog_bytes -= bytes;
 23	tin->backlog_packets -= packets;
 24	flow->backlog -= bytes;
 25	fq->backlog -= packets;
 26	fq->memory_usage -= truesize;
 27
 28	if (flow->backlog)
 29		return;
 30
 31	if (flow == &tin->default_flow) {
 32		list_del_init(&tin->tin_list);
 33		return;
 34	}
 35
 36	idx = flow - fq->flows;
 37	__clear_bit(idx, fq->flows_bitmap);
 38}
 39
 40static void fq_adjust_removal(struct fq *fq,
 41			      struct fq_flow *flow,
 42			      struct sk_buff *skb)
 43{
 44	__fq_adjust_removal(fq, flow, 1, skb->len, skb->truesize);
 45}
 46
 47static struct sk_buff *fq_flow_dequeue(struct fq *fq,
 48				       struct fq_flow *flow)
 49{
 50	struct sk_buff *skb;
 51
 52	lockdep_assert_held(&fq->lock);
 53
 54	skb = __skb_dequeue(&flow->queue);
 55	if (!skb)
 56		return NULL;
 57
 58	fq_adjust_removal(fq, flow, skb);
 59
 60	return skb;
 61}
 62
 63static int fq_flow_drop(struct fq *fq, struct fq_flow *flow,
 64			fq_skb_free_t free_func)
 65{
 66	unsigned int packets = 0, bytes = 0, truesize = 0;
 67	struct fq_tin *tin = flow->tin;
 68	struct sk_buff *skb;
 69	int pending;
 70
 71	lockdep_assert_held(&fq->lock);
 72
 73	pending = min_t(int, 32, skb_queue_len(&flow->queue) / 2);
 74	do {
 75		skb = __skb_dequeue(&flow->queue);
 76		if (!skb)
 77			break;
 78
 79		packets++;
 80		bytes += skb->len;
 81		truesize += skb->truesize;
 82		free_func(fq, tin, flow, skb);
 83	} while (packets < pending);
 84
 85	__fq_adjust_removal(fq, flow, packets, bytes, truesize);
 86
 87	return packets;
 88}
 89
 90static struct sk_buff *fq_tin_dequeue(struct fq *fq,
 91				      struct fq_tin *tin,
 92				      fq_tin_dequeue_t dequeue_func)
 93{
 94	struct fq_flow *flow;
 95	struct list_head *head;
 96	struct sk_buff *skb;
 97
 98	lockdep_assert_held(&fq->lock);
 99
100begin:
101	head = &tin->new_flows;
102	if (list_empty(head)) {
103		head = &tin->old_flows;
104		if (list_empty(head))
105			return NULL;
106	}
107
108	flow = list_first_entry(head, struct fq_flow, flowchain);
109
110	if (flow->deficit <= 0) {
111		flow->deficit += fq->quantum;
112		list_move_tail(&flow->flowchain,
113			       &tin->old_flows);
114		goto begin;
115	}
116
117	skb = dequeue_func(fq, tin, flow);
118	if (!skb) {
119		/* force a pass through old_flows to prevent starvation */
120		if ((head == &tin->new_flows) &&
121		    !list_empty(&tin->old_flows)) {
122			list_move_tail(&flow->flowchain, &tin->old_flows);
123		} else {
124			list_del_init(&flow->flowchain);
125			flow->tin = NULL;
126		}
127		goto begin;
128	}
129
130	flow->deficit -= skb->len;
131	tin->tx_bytes += skb->len;
132	tin->tx_packets++;
133
134	return skb;
135}
136
137static u32 fq_flow_idx(struct fq *fq, struct sk_buff *skb)
138{
139	u32 hash = skb_get_hash(skb);
140
141	return reciprocal_scale(hash, fq->flows_cnt);
142}
143
144static struct fq_flow *fq_flow_classify(struct fq *fq,
145					struct fq_tin *tin, u32 idx,
146					struct sk_buff *skb)
147{
148	struct fq_flow *flow;
149
150	lockdep_assert_held(&fq->lock);
151
152	flow = &fq->flows[idx];
153	if (flow->tin && flow->tin != tin) {
154		flow = &tin->default_flow;
155		tin->collisions++;
156		fq->collisions++;
157	}
158
159	if (!flow->tin)
160		tin->flows++;
161
162	return flow;
163}
164
165static struct fq_flow *fq_find_fattest_flow(struct fq *fq)
166{
167	struct fq_tin *tin;
168	struct fq_flow *flow = NULL;
169	u32 len = 0;
170	int i;
171
172	for_each_set_bit(i, fq->flows_bitmap, fq->flows_cnt) {
173		struct fq_flow *cur = &fq->flows[i];
174		unsigned int cur_len;
175
176		cur_len = cur->backlog;
177		if (cur_len <= len)
178			continue;
179
180		flow = cur;
181		len = cur_len;
182	}
183
184	list_for_each_entry(tin, &fq->tin_backlog, tin_list) {
185		unsigned int cur_len = tin->default_flow.backlog;
186
187		if (cur_len <= len)
188			continue;
189
190		flow = &tin->default_flow;
191		len = cur_len;
192	}
193
194	return flow;
195}
196
197static void fq_tin_enqueue(struct fq *fq,
198			   struct fq_tin *tin, u32 idx,
199			   struct sk_buff *skb,
200			   fq_skb_free_t free_func)
201{
202	struct fq_flow *flow;
203	bool oom;
204
205	lockdep_assert_held(&fq->lock);
206
207	flow = fq_flow_classify(fq, tin, idx, skb);
208
209	if (!flow->backlog) {
210		if (flow != &tin->default_flow)
211			__set_bit(idx, fq->flows_bitmap);
212		else if (list_empty(&tin->tin_list))
213			list_add(&tin->tin_list, &fq->tin_backlog);
214	}
215
216	flow->tin = tin;
217	flow->backlog += skb->len;
218	tin->backlog_bytes += skb->len;
219	tin->backlog_packets++;
220	fq->memory_usage += skb->truesize;
221	fq->backlog++;
222
223	if (list_empty(&flow->flowchain)) {
224		flow->deficit = fq->quantum;
225		list_add_tail(&flow->flowchain,
226			      &tin->new_flows);
227	}
228
229	__skb_queue_tail(&flow->queue, skb);
230	oom = (fq->memory_usage > fq->memory_limit);
231	while (fq->backlog > fq->limit || oom) {
232		flow = fq_find_fattest_flow(fq);
233		if (!flow)
234			return;
235
236		if (!fq_flow_drop(fq, flow, free_func))
237			return;
238
239		flow->tin->overlimit++;
240		fq->overlimit++;
241		if (oom) {
242			fq->overmemory++;
243			oom = (fq->memory_usage > fq->memory_limit);
244		}
245	}
246}
247
248static void fq_flow_filter(struct fq *fq,
249			   struct fq_flow *flow,
250			   fq_skb_filter_t filter_func,
251			   void *filter_data,
252			   fq_skb_free_t free_func)
253{
254	struct fq_tin *tin = flow->tin;
255	struct sk_buff *skb, *tmp;
256
257	lockdep_assert_held(&fq->lock);
258
259	skb_queue_walk_safe(&flow->queue, skb, tmp) {
260		if (!filter_func(fq, tin, flow, skb, filter_data))
261			continue;
262
263		__skb_unlink(skb, &flow->queue);
264		fq_adjust_removal(fq, flow, skb);
265		free_func(fq, tin, flow, skb);
266	}
267}
268
269static void fq_tin_filter(struct fq *fq,
270			  struct fq_tin *tin,
271			  fq_skb_filter_t filter_func,
272			  void *filter_data,
273			  fq_skb_free_t free_func)
274{
275	struct fq_flow *flow;
276
277	lockdep_assert_held(&fq->lock);
278
279	list_for_each_entry(flow, &tin->new_flows, flowchain)
280		fq_flow_filter(fq, flow, filter_func, filter_data, free_func);
281	list_for_each_entry(flow, &tin->old_flows, flowchain)
282		fq_flow_filter(fq, flow, filter_func, filter_data, free_func);
283}
284
285static void fq_flow_reset(struct fq *fq,
286			  struct fq_flow *flow,
287			  fq_skb_free_t free_func)
288{
289	struct fq_tin *tin = flow->tin;
290	struct sk_buff *skb;
291
292	while ((skb = fq_flow_dequeue(fq, flow)))
293		free_func(fq, tin, flow, skb);
294
295	if (!list_empty(&flow->flowchain)) {
296		list_del_init(&flow->flowchain);
297		if (list_empty(&tin->new_flows) &&
298		    list_empty(&tin->old_flows))
299			list_del_init(&tin->tin_list);
300	}
301
302	flow->tin = NULL;
303
304	WARN_ON_ONCE(flow->backlog);
305}
306
307static void fq_tin_reset(struct fq *fq,
308			 struct fq_tin *tin,
309			 fq_skb_free_t free_func)
310{
311	struct list_head *head;
312	struct fq_flow *flow;
313
314	for (;;) {
315		head = &tin->new_flows;
316		if (list_empty(head)) {
317			head = &tin->old_flows;
318			if (list_empty(head))
319				break;
320		}
321
322		flow = list_first_entry(head, struct fq_flow, flowchain);
323		fq_flow_reset(fq, flow, free_func);
324	}
325
326	WARN_ON_ONCE(!list_empty(&tin->tin_list));
327	WARN_ON_ONCE(tin->backlog_bytes);
328	WARN_ON_ONCE(tin->backlog_packets);
329}
330
331static void fq_flow_init(struct fq_flow *flow)
332{
333	INIT_LIST_HEAD(&flow->flowchain);
334	__skb_queue_head_init(&flow->queue);
335}
336
337static void fq_tin_init(struct fq_tin *tin)
338{
339	INIT_LIST_HEAD(&tin->new_flows);
340	INIT_LIST_HEAD(&tin->old_flows);
341	INIT_LIST_HEAD(&tin->tin_list);
342	fq_flow_init(&tin->default_flow);
343}
344
345static int fq_init(struct fq *fq, int flows_cnt)
346{
347	int i;
348
349	memset(fq, 0, sizeof(fq[0]));
350	spin_lock_init(&fq->lock);
351	INIT_LIST_HEAD(&fq->tin_backlog);
352	fq->flows_cnt = max_t(u32, flows_cnt, 1);
353	fq->quantum = 300;
354	fq->limit = 8192;
355	fq->memory_limit = 16 << 20; /* 16 MBytes */
356
357	fq->flows = kvcalloc(fq->flows_cnt, sizeof(fq->flows[0]), GFP_KERNEL);
358	if (!fq->flows)
359		return -ENOMEM;
360
361	fq->flows_bitmap = kcalloc(BITS_TO_LONGS(fq->flows_cnt), sizeof(long),
362				   GFP_KERNEL);
363	if (!fq->flows_bitmap) {
364		kvfree(fq->flows);
365		fq->flows = NULL;
366		return -ENOMEM;
367	}
368
369	for (i = 0; i < fq->flows_cnt; i++)
370		fq_flow_init(&fq->flows[i]);
371
372	return 0;
373}
374
375static void fq_reset(struct fq *fq,
376		     fq_skb_free_t free_func)
377{
378	int i;
379
380	for (i = 0; i < fq->flows_cnt; i++)
381		fq_flow_reset(fq, &fq->flows[i], free_func);
382
383	kvfree(fq->flows);
384	fq->flows = NULL;
385
386	kfree(fq->flows_bitmap);
387	fq->flows_bitmap = NULL;
388}
389
390#endif