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