1// SPDX-License-Identifier: GPL-2.0-only
2/*
3 * O(1) TX queue with built-in allocator for ST-Ericsson CW1200 drivers
4 *
5 * Copyright (c) 2010, ST-Ericsson
6 * Author: Dmitry Tarnyagin <dmitry.tarnyagin@lockless.no>
7 */
8
9#include <net/mac80211.h>
10#include <linux/sched.h>
11#include "queue.h"
12#include "cw1200.h"
13#include "debug.h"
14
15/* private */ struct cw1200_queue_item
16{
17	struct list_head	head;
18	struct sk_buff		*skb;
19	u32			packet_id;
20	unsigned long		queue_timestamp;
21	unsigned long		xmit_timestamp;
22	struct cw1200_txpriv	txpriv;
23	u8			generation;
24};
25
26static inline void __cw1200_queue_lock(struct cw1200_queue *queue)
27{
28	struct cw1200_queue_stats *stats = queue->stats;
29	if (queue->tx_locked_cnt++ == 0) {
30		pr_debug("[TX] Queue %d is locked.\n",
31			 queue->queue_id);
32		ieee80211_stop_queue(stats->priv->hw, queue->queue_id);
33	}
34}
35
36static inline void __cw1200_queue_unlock(struct cw1200_queue *queue)
37{
38	struct cw1200_queue_stats *stats = queue->stats;
39	BUG_ON(!queue->tx_locked_cnt);
40	if (--queue->tx_locked_cnt == 0) {
41		pr_debug("[TX] Queue %d is unlocked.\n",
42			 queue->queue_id);
43		ieee80211_wake_queue(stats->priv->hw, queue->queue_id);
44	}
45}
46
47static inline void cw1200_queue_parse_id(u32 packet_id, u8 *queue_generation,
48					 u8 *queue_id, u8 *item_generation,
49					 u8 *item_id)
50{
51	*item_id		= (packet_id >>  0) & 0xFF;
52	*item_generation	= (packet_id >>  8) & 0xFF;
53	*queue_id		= (packet_id >> 16) & 0xFF;
54	*queue_generation	= (packet_id >> 24) & 0xFF;
55}
56
57static inline u32 cw1200_queue_mk_packet_id(u8 queue_generation, u8 queue_id,
58					    u8 item_generation, u8 item_id)
59{
60	return ((u32)item_id << 0) |
61		((u32)item_generation << 8) |
62		((u32)queue_id << 16) |
63		((u32)queue_generation << 24);
64}
65
66static void cw1200_queue_post_gc(struct cw1200_queue_stats *stats,
67				 struct list_head *gc_list)
68{
69	struct cw1200_queue_item *item, *tmp;
70
71	list_for_each_entry_safe(item, tmp, gc_list, head) {
72		list_del(&item->head);
73		stats->skb_dtor(stats->priv, item->skb, &item->txpriv);
74		kfree(item);
75	}
76}
77
78static void cw1200_queue_register_post_gc(struct list_head *gc_list,
79					  struct cw1200_queue_item *item)
80{
81	struct cw1200_queue_item *gc_item;
82	gc_item = kmemdup(item, sizeof(struct cw1200_queue_item),
83			GFP_ATOMIC);
84	BUG_ON(!gc_item);
85	list_add_tail(&gc_item->head, gc_list);
86}
87
88static void __cw1200_queue_gc(struct cw1200_queue *queue,
89			      struct list_head *head,
90			      bool unlock)
91{
92	struct cw1200_queue_stats *stats = queue->stats;
93	struct cw1200_queue_item *item = NULL, *tmp;
94	bool wakeup_stats = false;
95
96	list_for_each_entry_safe(item, tmp, &queue->queue, head) {
97		if (jiffies - item->queue_timestamp < queue->ttl)
98			break;
99		--queue->num_queued;
100		--queue->link_map_cache[item->txpriv.link_id];
101		spin_lock_bh(&stats->lock);
102		--stats->num_queued;
103		if (!--stats->link_map_cache[item->txpriv.link_id])
104			wakeup_stats = true;
105		spin_unlock_bh(&stats->lock);
106		cw1200_debug_tx_ttl(stats->priv);
107		cw1200_queue_register_post_gc(head, item);
108		item->skb = NULL;
109		list_move_tail(&item->head, &queue->free_pool);
110	}
111
112	if (wakeup_stats)
113		wake_up(&stats->wait_link_id_empty);
114
115	if (queue->overfull) {
116		if (queue->num_queued <= (queue->capacity >> 1)) {
117			queue->overfull = false;
118			if (unlock)
119				__cw1200_queue_unlock(queue);
120		} else if (item) {
121			unsigned long tmo = item->queue_timestamp + queue->ttl;
122			mod_timer(&queue->gc, tmo);
123			cw1200_pm_stay_awake(&stats->priv->pm_state,
124					     tmo - jiffies);
125		}
126	}
127}
128
129static void cw1200_queue_gc(struct timer_list *t)
130{
131	LIST_HEAD(list);
132	struct cw1200_queue *queue =
133		from_timer(queue, t, gc);
134
135	spin_lock_bh(&queue->lock);
136	__cw1200_queue_gc(queue, &list, true);
137	spin_unlock_bh(&queue->lock);
138	cw1200_queue_post_gc(queue->stats, &list);
139}
140
141int cw1200_queue_stats_init(struct cw1200_queue_stats *stats,
142			    size_t map_capacity,
143			    cw1200_queue_skb_dtor_t skb_dtor,
144			    struct cw1200_common *priv)
145{
146	memset(stats, 0, sizeof(*stats));
147	stats->map_capacity = map_capacity;
148	stats->skb_dtor = skb_dtor;
149	stats->priv = priv;
150	spin_lock_init(&stats->lock);
151	init_waitqueue_head(&stats->wait_link_id_empty);
152
153	stats->link_map_cache = kcalloc(map_capacity, sizeof(int),
154					GFP_KERNEL);
155	if (!stats->link_map_cache)
156		return -ENOMEM;
157
158	return 0;
159}
160
161int cw1200_queue_init(struct cw1200_queue *queue,
162		      struct cw1200_queue_stats *stats,
163		      u8 queue_id,
164		      size_t capacity,
165		      unsigned long ttl)
166{
167	size_t i;
168
169	memset(queue, 0, sizeof(*queue));
170	queue->stats = stats;
171	queue->capacity = capacity;
172	queue->queue_id = queue_id;
173	queue->ttl = ttl;
174	INIT_LIST_HEAD(&queue->queue);
175	INIT_LIST_HEAD(&queue->pending);
176	INIT_LIST_HEAD(&queue->free_pool);
177	spin_lock_init(&queue->lock);
178	timer_setup(&queue->gc, cw1200_queue_gc, 0);
179
180	queue->pool = kcalloc(capacity, sizeof(struct cw1200_queue_item),
181			      GFP_KERNEL);
182	if (!queue->pool)
183		return -ENOMEM;
184
185	queue->link_map_cache = kcalloc(stats->map_capacity, sizeof(int),
186					GFP_KERNEL);
187	if (!queue->link_map_cache) {
188		kfree(queue->pool);
189		queue->pool = NULL;
190		return -ENOMEM;
191	}
192
193	for (i = 0; i < capacity; ++i)
194		list_add_tail(&queue->pool[i].head, &queue->free_pool);
195
196	return 0;
197}
198
199int cw1200_queue_clear(struct cw1200_queue *queue)
200{
201	int i;
202	LIST_HEAD(gc_list);
203	struct cw1200_queue_stats *stats = queue->stats;
204	struct cw1200_queue_item *item, *tmp;
205
206	spin_lock_bh(&queue->lock);
207	queue->generation++;
208	list_splice_tail_init(&queue->queue, &queue->pending);
209	list_for_each_entry_safe(item, tmp, &queue->pending, head) {
210		WARN_ON(!item->skb);
211		cw1200_queue_register_post_gc(&gc_list, item);
212		item->skb = NULL;
213		list_move_tail(&item->head, &queue->free_pool);
214	}
215	queue->num_queued = 0;
216	queue->num_pending = 0;
217
218	spin_lock_bh(&stats->lock);
219	for (i = 0; i < stats->map_capacity; ++i) {
220		stats->num_queued -= queue->link_map_cache[i];
221		stats->link_map_cache[i] -= queue->link_map_cache[i];
222		queue->link_map_cache[i] = 0;
223	}
224	spin_unlock_bh(&stats->lock);
225	if (queue->overfull) {
226		queue->overfull = false;
227		__cw1200_queue_unlock(queue);
228	}
229	spin_unlock_bh(&queue->lock);
230	wake_up(&stats->wait_link_id_empty);
231	cw1200_queue_post_gc(stats, &gc_list);
232	return 0;
233}
234
235void cw1200_queue_stats_deinit(struct cw1200_queue_stats *stats)
236{
237	kfree(stats->link_map_cache);
238	stats->link_map_cache = NULL;
239}
240
241void cw1200_queue_deinit(struct cw1200_queue *queue)
242{
243	cw1200_queue_clear(queue);
244	del_timer_sync(&queue->gc);
245	INIT_LIST_HEAD(&queue->free_pool);
246	kfree(queue->pool);
247	kfree(queue->link_map_cache);
248	queue->pool = NULL;
249	queue->link_map_cache = NULL;
250	queue->capacity = 0;
251}
252
253size_t cw1200_queue_get_num_queued(struct cw1200_queue *queue,
254				   u32 link_id_map)
255{
256	size_t ret;
257	int i, bit;
258	size_t map_capacity = queue->stats->map_capacity;
259
260	if (!link_id_map)
261		return 0;
262
263	spin_lock_bh(&queue->lock);
264	if (link_id_map == (u32)-1) {
265		ret = queue->num_queued - queue->num_pending;
266	} else {
267		ret = 0;
268		for (i = 0, bit = 1; i < map_capacity; ++i, bit <<= 1) {
269			if (link_id_map & bit)
270				ret += queue->link_map_cache[i];
271		}
272	}
273	spin_unlock_bh(&queue->lock);
274	return ret;
275}
276
277int cw1200_queue_put(struct cw1200_queue *queue,
278		     struct sk_buff *skb,
279		     struct cw1200_txpriv *txpriv)
280{
281	int ret = 0;
282	struct cw1200_queue_stats *stats = queue->stats;
283
284	if (txpriv->link_id >= queue->stats->map_capacity)
285		return -EINVAL;
286
287	spin_lock_bh(&queue->lock);
288	if (!WARN_ON(list_empty(&queue->free_pool))) {
289		struct cw1200_queue_item *item = list_first_entry(
290			&queue->free_pool, struct cw1200_queue_item, head);
291		BUG_ON(item->skb);
292
293		list_move_tail(&item->head, &queue->queue);
294		item->skb = skb;
295		item->txpriv = *txpriv;
296		item->generation = 0;
297		item->packet_id = cw1200_queue_mk_packet_id(queue->generation,
298							    queue->queue_id,
299							    item->generation,
300							    item - queue->pool);
301		item->queue_timestamp = jiffies;
302
303		++queue->num_queued;
304		++queue->link_map_cache[txpriv->link_id];
305
306		spin_lock_bh(&stats->lock);
307		++stats->num_queued;
308		++stats->link_map_cache[txpriv->link_id];
309		spin_unlock_bh(&stats->lock);
310
311		/* TX may happen in parallel sometimes.
312		 * Leave extra queue slots so we don't overflow.
313		 */
314		if (queue->overfull == false &&
315		    queue->num_queued >=
316		    (queue->capacity - (num_present_cpus() - 1))) {
317			queue->overfull = true;
318			__cw1200_queue_lock(queue);
319			mod_timer(&queue->gc, jiffies);
320		}
321	} else {
322		ret = -ENOENT;
323	}
324	spin_unlock_bh(&queue->lock);
325	return ret;
326}
327
328int cw1200_queue_get(struct cw1200_queue *queue,
329		     u32 link_id_map,
330		     struct wsm_tx **tx,
331		     struct ieee80211_tx_info **tx_info,
332		     const struct cw1200_txpriv **txpriv)
333{
334	int ret = -ENOENT;
335	struct cw1200_queue_item *item;
336	struct cw1200_queue_stats *stats = queue->stats;
337	bool wakeup_stats = false;
338
339	spin_lock_bh(&queue->lock);
340	list_for_each_entry(item, &queue->queue, head) {
341		if (link_id_map & BIT(item->txpriv.link_id)) {
342			ret = 0;
343			break;
344		}
345	}
346
347	if (!WARN_ON(ret)) {
348		*tx = (struct wsm_tx *)item->skb->data;
349		*tx_info = IEEE80211_SKB_CB(item->skb);
350		*txpriv = &item->txpriv;
351		(*tx)->packet_id = item->packet_id;
352		list_move_tail(&item->head, &queue->pending);
353		++queue->num_pending;
354		--queue->link_map_cache[item->txpriv.link_id];
355		item->xmit_timestamp = jiffies;
356
357		spin_lock_bh(&stats->lock);
358		--stats->num_queued;
359		if (!--stats->link_map_cache[item->txpriv.link_id])
360			wakeup_stats = true;
361		spin_unlock_bh(&stats->lock);
362	}
363	spin_unlock_bh(&queue->lock);
364	if (wakeup_stats)
365		wake_up(&stats->wait_link_id_empty);
366	return ret;
367}
368
369int cw1200_queue_requeue(struct cw1200_queue *queue, u32 packet_id)
370{
371	int ret = 0;
372	u8 queue_generation, queue_id, item_generation, item_id;
373	struct cw1200_queue_item *item;
374	struct cw1200_queue_stats *stats = queue->stats;
375
376	cw1200_queue_parse_id(packet_id, &queue_generation, &queue_id,
377			      &item_generation, &item_id);
378
379	item = &queue->pool[item_id];
380
381	spin_lock_bh(&queue->lock);
382	BUG_ON(queue_id != queue->queue_id);
383	if (queue_generation != queue->generation) {
384		ret = -ENOENT;
385	} else if (item_id >= (unsigned) queue->capacity) {
386		WARN_ON(1);
387		ret = -EINVAL;
388	} else if (item->generation != item_generation) {
389		WARN_ON(1);
390		ret = -ENOENT;
391	} else {
392		--queue->num_pending;
393		++queue->link_map_cache[item->txpriv.link_id];
394
395		spin_lock_bh(&stats->lock);
396		++stats->num_queued;
397		++stats->link_map_cache[item->txpriv.link_id];
398		spin_unlock_bh(&stats->lock);
399
400		item->generation = ++item_generation;
401		item->packet_id = cw1200_queue_mk_packet_id(queue_generation,
402							    queue_id,
403							    item_generation,
404							    item_id);
405		list_move(&item->head, &queue->queue);
406	}
407	spin_unlock_bh(&queue->lock);
408	return ret;
409}
410
411int cw1200_queue_requeue_all(struct cw1200_queue *queue)
412{
413	struct cw1200_queue_item *item, *tmp;
414	struct cw1200_queue_stats *stats = queue->stats;
415	spin_lock_bh(&queue->lock);
416
417	list_for_each_entry_safe_reverse(item, tmp, &queue->pending, head) {
418		--queue->num_pending;
419		++queue->link_map_cache[item->txpriv.link_id];
420
421		spin_lock_bh(&stats->lock);
422		++stats->num_queued;
423		++stats->link_map_cache[item->txpriv.link_id];
424		spin_unlock_bh(&stats->lock);
425
426		++item->generation;
427		item->packet_id = cw1200_queue_mk_packet_id(queue->generation,
428							    queue->queue_id,
429							    item->generation,
430							    item - queue->pool);
431		list_move(&item->head, &queue->queue);
432	}
433	spin_unlock_bh(&queue->lock);
434
435	return 0;
436}
437
438int cw1200_queue_remove(struct cw1200_queue *queue, u32 packet_id)
439{
440	int ret = 0;
441	u8 queue_generation, queue_id, item_generation, item_id;
442	struct cw1200_queue_item *item;
443	struct cw1200_queue_stats *stats = queue->stats;
444	struct sk_buff *gc_skb = NULL;
445	struct cw1200_txpriv gc_txpriv;
446
447	cw1200_queue_parse_id(packet_id, &queue_generation, &queue_id,
448			      &item_generation, &item_id);
449
450	item = &queue->pool[item_id];
451
452	spin_lock_bh(&queue->lock);
453	BUG_ON(queue_id != queue->queue_id);
454	if (queue_generation != queue->generation) {
455		ret = -ENOENT;
456	} else if (item_id >= (unsigned) queue->capacity) {
457		WARN_ON(1);
458		ret = -EINVAL;
459	} else if (item->generation != item_generation) {
460		WARN_ON(1);
461		ret = -ENOENT;
462	} else {
463		gc_txpriv = item->txpriv;
464		gc_skb = item->skb;
465		item->skb = NULL;
466		--queue->num_pending;
467		--queue->num_queued;
468		++queue->num_sent;
469		++item->generation;
470		/* Do not use list_move_tail here, but list_move:
471		 * try to utilize cache row.
472		 */
473		list_move(&item->head, &queue->free_pool);
474
475		if (queue->overfull &&
476		    (queue->num_queued <= (queue->capacity >> 1))) {
477			queue->overfull = false;
478			__cw1200_queue_unlock(queue);
479		}
480	}
481	spin_unlock_bh(&queue->lock);
482
483	if (gc_skb)
484		stats->skb_dtor(stats->priv, gc_skb, &gc_txpriv);
485
486	return ret;
487}
488
489int cw1200_queue_get_skb(struct cw1200_queue *queue, u32 packet_id,
490			 struct sk_buff **skb,
491			 const struct cw1200_txpriv **txpriv)
492{
493	int ret = 0;
494	u8 queue_generation, queue_id, item_generation, item_id;
495	struct cw1200_queue_item *item;
496	cw1200_queue_parse_id(packet_id, &queue_generation, &queue_id,
497			      &item_generation, &item_id);
498
499	item = &queue->pool[item_id];
500
501	spin_lock_bh(&queue->lock);
502	BUG_ON(queue_id != queue->queue_id);
503	if (queue_generation != queue->generation) {
504		ret = -ENOENT;
505	} else if (item_id >= (unsigned) queue->capacity) {
506		WARN_ON(1);
507		ret = -EINVAL;
508	} else if (item->generation != item_generation) {
509		WARN_ON(1);
510		ret = -ENOENT;
511	} else {
512		*skb = item->skb;
513		*txpriv = &item->txpriv;
514	}
515	spin_unlock_bh(&queue->lock);
516	return ret;
517}
518
519void cw1200_queue_lock(struct cw1200_queue *queue)
520{
521	spin_lock_bh(&queue->lock);
522	__cw1200_queue_lock(queue);
523	spin_unlock_bh(&queue->lock);
524}
525
526void cw1200_queue_unlock(struct cw1200_queue *queue)
527{
528	spin_lock_bh(&queue->lock);
529	__cw1200_queue_unlock(queue);
530	spin_unlock_bh(&queue->lock);
531}
532
533bool cw1200_queue_get_xmit_timestamp(struct cw1200_queue *queue,
534				     unsigned long *timestamp,
535				     u32 pending_frame_id)
536{
537	struct cw1200_queue_item *item;
538	bool ret;
539
540	spin_lock_bh(&queue->lock);
541	ret = !list_empty(&queue->pending);
542	if (ret) {
543		list_for_each_entry(item, &queue->pending, head) {
544			if (item->packet_id != pending_frame_id)
545				if (time_before(item->xmit_timestamp,
546						*timestamp))
547					*timestamp = item->xmit_timestamp;
548		}
549	}
550	spin_unlock_bh(&queue->lock);
551	return ret;
552}
553
554bool cw1200_queue_stats_is_empty(struct cw1200_queue_stats *stats,
555				 u32 link_id_map)
556{
557	bool empty = true;
558
559	spin_lock_bh(&stats->lock);
560	if (link_id_map == (u32)-1) {
561		empty = stats->num_queued == 0;
562	} else {
563		int i;
564		for (i = 0; i < stats->map_capacity; ++i) {
565			if (link_id_map & BIT(i)) {
566				if (stats->link_map_cache[i]) {
567					empty = false;
568					break;
569				}
570			}
571		}
572	}
573	spin_unlock_bh(&stats->lock);
574
575	return empty;
576}
577