cachepc-linux

Fork of AMDESE/linux with modifications for CachePC side-channel attack
git clone https://git.sinitax.com/sinitax/cachepc-linux
Log | Files | Refs | README | LICENSE | sfeed.txt

queue.c (14691B)


      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}