txrx.c

   1// SPDX-License-Identifier: GPL-2.0-only
   2/*
   3 * Datapath implementation for ST-Ericsson CW1200 mac80211 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/etherdevice.h>
  11#include <linux/skbuff.h>
  12
  13#include "cw1200.h"
  14#include "wsm.h"
  15#include "bh.h"
  16#include "sta.h"
  17#include "debug.h"
  18
  19#define CW1200_INVALID_RATE_ID (0xFF)
  20
  21static int cw1200_handle_action_rx(struct cw1200_common *priv,
  22				   struct sk_buff *skb);
  23static const struct ieee80211_rate *
  24cw1200_get_tx_rate(const struct cw1200_common *priv,
  25		   const struct ieee80211_tx_rate *rate);
  26
  27/* ******************************************************************** */
  28/* TX queue lock / unlock						*/
  29
  30static inline void cw1200_tx_queues_lock(struct cw1200_common *priv)
  31{
  32	int i;
  33	for (i = 0; i < 4; ++i)
  34		cw1200_queue_lock(&priv->tx_queue[i]);
  35}
  36
  37static inline void cw1200_tx_queues_unlock(struct cw1200_common *priv)
  38{
  39	int i;
  40	for (i = 0; i < 4; ++i)
  41		cw1200_queue_unlock(&priv->tx_queue[i]);
  42}
  43
  44/* ******************************************************************** */
  45/* TX policy cache implementation					*/
  46
  47static void tx_policy_dump(struct tx_policy *policy)
  48{
  49	pr_debug("[TX policy] %.1X%.1X%.1X%.1X%.1X%.1X%.1X%.1X %.1X%.1X%.1X%.1X%.1X%.1X%.1X%.1X %.1X%.1X%.1X%.1X%.1X%.1X%.1X%.1X: %d\n",
  50		 policy->raw[0] & 0x0F,  policy->raw[0] >> 4,
  51		 policy->raw[1] & 0x0F,  policy->raw[1] >> 4,
  52		 policy->raw[2] & 0x0F,  policy->raw[2] >> 4,
  53		 policy->raw[3] & 0x0F,  policy->raw[3] >> 4,
  54		 policy->raw[4] & 0x0F,  policy->raw[4] >> 4,
  55		 policy->raw[5] & 0x0F,  policy->raw[5] >> 4,
  56		 policy->raw[6] & 0x0F,  policy->raw[6] >> 4,
  57		 policy->raw[7] & 0x0F,  policy->raw[7] >> 4,
  58		 policy->raw[8] & 0x0F,  policy->raw[8] >> 4,
  59		 policy->raw[9] & 0x0F,  policy->raw[9] >> 4,
  60		 policy->raw[10] & 0x0F,  policy->raw[10] >> 4,
  61		 policy->raw[11] & 0x0F,  policy->raw[11] >> 4,
  62		 policy->defined);
  63}
  64
  65static void tx_policy_build(const struct cw1200_common *priv,
  66	/* [out] */ struct tx_policy *policy,
  67	struct ieee80211_tx_rate *rates, size_t count)
  68{
  69	int i, j;
  70	unsigned limit = priv->short_frame_max_tx_count;
  71	unsigned total = 0;
  72	BUG_ON(rates[0].idx < 0);
  73	memset(policy, 0, sizeof(*policy));
  74
  75	/* Sort rates in descending order. */
  76	for (i = 1; i < count; ++i) {
  77		if (rates[i].idx < 0) {
  78			count = i;
  79			break;
  80		}
  81		if (rates[i].idx > rates[i - 1].idx) {
  82			struct ieee80211_tx_rate tmp = rates[i - 1];
  83			rates[i - 1] = rates[i];
  84			rates[i] = tmp;
  85		}
  86	}
  87
  88	/* Eliminate duplicates. */
  89	total = rates[0].count;
  90	for (i = 0, j = 1; j < count; ++j) {
  91		if (rates[j].idx == rates[i].idx) {
  92			rates[i].count += rates[j].count;
  93		} else if (rates[j].idx > rates[i].idx) {
  94			break;
  95		} else {
  96			++i;
  97			if (i != j)
  98				rates[i] = rates[j];
  99		}
 100		total += rates[j].count;
 101	}
 102	count = i + 1;
 103
 104	/* Re-fill policy trying to keep every requested rate and with
 105	 * respect to the global max tx retransmission count.
 106	 */
 107	if (limit < count)
 108		limit = count;
 109	if (total > limit) {
 110		for (i = 0; i < count; ++i) {
 111			int left = count - i - 1;
 112			if (rates[i].count > limit - left)
 113				rates[i].count = limit - left;
 114			limit -= rates[i].count;
 115		}
 116	}
 117
 118	/* HACK!!! Device has problems (at least) switching from
 119	 * 54Mbps CTS to 1Mbps. This switch takes enormous amount
 120	 * of time (100-200 ms), leading to valuable throughput drop.
 121	 * As a workaround, additional g-rates are injected to the
 122	 * policy.
 123	 */
 124	if (count == 2 && !(rates[0].flags & IEEE80211_TX_RC_MCS) &&
 125	    rates[0].idx > 4 && rates[0].count > 2 &&
 126	    rates[1].idx < 2) {
 127		int mid_rate = (rates[0].idx + 4) >> 1;
 128
 129		/* Decrease number of retries for the initial rate */
 130		rates[0].count -= 2;
 131
 132		if (mid_rate != 4) {
 133			/* Keep fallback rate at 1Mbps. */
 134			rates[3] = rates[1];
 135
 136			/* Inject 1 transmission on lowest g-rate */
 137			rates[2].idx = 4;
 138			rates[2].count = 1;
 139			rates[2].flags = rates[1].flags;
 140
 141			/* Inject 1 transmission on mid-rate */
 142			rates[1].idx = mid_rate;
 143			rates[1].count = 1;
 144
 145			/* Fallback to 1 Mbps is a really bad thing,
 146			 * so let's try to increase probability of
 147			 * successful transmission on the lowest g rate
 148			 * even more
 149			 */
 150			if (rates[0].count >= 3) {
 151				--rates[0].count;
 152				++rates[2].count;
 153			}
 154
 155			/* Adjust amount of rates defined */
 156			count += 2;
 157		} else {
 158			/* Keep fallback rate at 1Mbps. */
 159			rates[2] = rates[1];
 160
 161			/* Inject 2 transmissions on lowest g-rate */
 162			rates[1].idx = 4;
 163			rates[1].count = 2;
 164
 165			/* Adjust amount of rates defined */
 166			count += 1;
 167		}
 168	}
 169
 170	policy->defined = cw1200_get_tx_rate(priv, &rates[0])->hw_value + 1;
 171
 172	for (i = 0; i < count; ++i) {
 173		register unsigned rateid, off, shift, retries;
 174
 175		rateid = cw1200_get_tx_rate(priv, &rates[i])->hw_value;
 176		off = rateid >> 3;		/* eq. rateid / 8 */
 177		shift = (rateid & 0x07) << 2;	/* eq. (rateid % 8) * 4 */
 178
 179		retries = rates[i].count;
 180		if (retries > 0x0F) {
 181			rates[i].count = 0x0f;
 182			retries = 0x0F;
 183		}
 184		policy->tbl[off] |= __cpu_to_le32(retries << shift);
 185		policy->retry_count += retries;
 186	}
 187
 188	pr_debug("[TX policy] Policy (%zu): %d:%d, %d:%d, %d:%d, %d:%d\n",
 189		 count,
 190		 rates[0].idx, rates[0].count,
 191		 rates[1].idx, rates[1].count,
 192		 rates[2].idx, rates[2].count,
 193		 rates[3].idx, rates[3].count);
 194}
 195
 196static inline bool tx_policy_is_equal(const struct tx_policy *wanted,
 197					const struct tx_policy *cached)
 198{
 199	size_t count = wanted->defined >> 1;
 200	if (wanted->defined > cached->defined)
 201		return false;
 202	if (count) {
 203		if (memcmp(wanted->raw, cached->raw, count))
 204			return false;
 205	}
 206	if (wanted->defined & 1) {
 207		if ((wanted->raw[count] & 0x0F) != (cached->raw[count] & 0x0F))
 208			return false;
 209	}
 210	return true;
 211}
 212
 213static int tx_policy_find(struct tx_policy_cache *cache,
 214				const struct tx_policy *wanted)
 215{
 216	/* O(n) complexity. Not so good, but there's only 8 entries in
 217	 * the cache.
 218	 * Also lru helps to reduce search time.
 219	 */
 220	struct tx_policy_cache_entry *it;
 221	/* First search for policy in "used" list */
 222	list_for_each_entry(it, &cache->used, link) {
 223		if (tx_policy_is_equal(wanted, &it->policy))
 224			return it - cache->cache;
 225	}
 226	/* Then - in "free list" */
 227	list_for_each_entry(it, &cache->free, link) {
 228		if (tx_policy_is_equal(wanted, &it->policy))
 229			return it - cache->cache;
 230	}
 231	return -1;
 232}
 233
 234static inline void tx_policy_use(struct tx_policy_cache *cache,
 235				 struct tx_policy_cache_entry *entry)
 236{
 237	++entry->policy.usage_count;
 238	list_move(&entry->link, &cache->used);
 239}
 240
 241static inline int tx_policy_release(struct tx_policy_cache *cache,
 242				    struct tx_policy_cache_entry *entry)
 243{
 244	int ret = --entry->policy.usage_count;
 245	if (!ret)
 246		list_move(&entry->link, &cache->free);
 247	return ret;
 248}
 249
 250void tx_policy_clean(struct cw1200_common *priv)
 251{
 252	int idx, locked;
 253	struct tx_policy_cache *cache = &priv->tx_policy_cache;
 254	struct tx_policy_cache_entry *entry;
 255
 256	cw1200_tx_queues_lock(priv);
 257	spin_lock_bh(&cache->lock);
 258	locked = list_empty(&cache->free);
 259
 260	for (idx = 0; idx < TX_POLICY_CACHE_SIZE; idx++) {
 261		entry = &cache->cache[idx];
 262		/* Policy usage count should be 0 at this time as all queues
 263		   should be empty
 264		 */
 265		if (WARN_ON(entry->policy.usage_count)) {
 266			entry->policy.usage_count = 0;
 267			list_move(&entry->link, &cache->free);
 268		}
 269		memset(&entry->policy, 0, sizeof(entry->policy));
 270	}
 271	if (locked)
 272		cw1200_tx_queues_unlock(priv);
 273
 274	cw1200_tx_queues_unlock(priv);
 275	spin_unlock_bh(&cache->lock);
 276}
 277
 278/* ******************************************************************** */
 279/* External TX policy cache API						*/
 280
 281void tx_policy_init(struct cw1200_common *priv)
 282{
 283	struct tx_policy_cache *cache = &priv->tx_policy_cache;
 284	int i;
 285
 286	memset(cache, 0, sizeof(*cache));
 287
 288	spin_lock_init(&cache->lock);
 289	INIT_LIST_HEAD(&cache->used);
 290	INIT_LIST_HEAD(&cache->free);
 291
 292	for (i = 0; i < TX_POLICY_CACHE_SIZE; ++i)
 293		list_add(&cache->cache[i].link, &cache->free);
 294}
 295
 296static int tx_policy_get(struct cw1200_common *priv,
 297		  struct ieee80211_tx_rate *rates,
 298		  size_t count, bool *renew)
 299{
 300	int idx;
 301	struct tx_policy_cache *cache = &priv->tx_policy_cache;
 302	struct tx_policy wanted;
 303
 304	tx_policy_build(priv, &wanted, rates, count);
 305
 306	spin_lock_bh(&cache->lock);
 307	if (WARN_ON_ONCE(list_empty(&cache->free))) {
 308		spin_unlock_bh(&cache->lock);
 309		return CW1200_INVALID_RATE_ID;
 310	}
 311	idx = tx_policy_find(cache, &wanted);
 312	if (idx >= 0) {
 313		pr_debug("[TX policy] Used TX policy: %d\n", idx);
 314		*renew = false;
 315	} else {
 316		struct tx_policy_cache_entry *entry;
 317		*renew = true;
 318		/* If policy is not found create a new one
 319		 * using the oldest entry in "free" list
 320		 */
 321		entry = list_entry(cache->free.prev,
 322			struct tx_policy_cache_entry, link);
 323		entry->policy = wanted;
 324		idx = entry - cache->cache;
 325		pr_debug("[TX policy] New TX policy: %d\n", idx);
 326		tx_policy_dump(&entry->policy);
 327	}
 328	tx_policy_use(cache, &cache->cache[idx]);
 329	if (list_empty(&cache->free)) {
 330		/* Lock TX queues. */
 331		cw1200_tx_queues_lock(priv);
 332	}
 333	spin_unlock_bh(&cache->lock);
 334	return idx;
 335}
 336
 337static void tx_policy_put(struct cw1200_common *priv, int idx)
 338{
 339	int usage, locked;
 340	struct tx_policy_cache *cache = &priv->tx_policy_cache;
 341
 342	spin_lock_bh(&cache->lock);
 343	locked = list_empty(&cache->free);
 344	usage = tx_policy_release(cache, &cache->cache[idx]);
 345	if (locked && !usage) {
 346		/* Unlock TX queues. */
 347		cw1200_tx_queues_unlock(priv);
 348	}
 349	spin_unlock_bh(&cache->lock);
 350}
 351
 352static int tx_policy_upload(struct cw1200_common *priv)
 353{
 354	struct tx_policy_cache *cache = &priv->tx_policy_cache;
 355	int i;
 356	struct wsm_set_tx_rate_retry_policy arg = {
 357		.num = 0,
 358	};
 359	spin_lock_bh(&cache->lock);
 360
 361	/* Upload only modified entries. */
 362	for (i = 0; i < TX_POLICY_CACHE_SIZE; ++i) {
 363		struct tx_policy *src = &cache->cache[i].policy;
 364		if (src->retry_count && !src->uploaded) {
 365			struct wsm_tx_rate_retry_policy *dst =
 366				&arg.tbl[arg.num];
 367			dst->index = i;
 368			dst->short_retries = priv->short_frame_max_tx_count;
 369			dst->long_retries = priv->long_frame_max_tx_count;
 370
 371			dst->flags = WSM_TX_RATE_POLICY_FLAG_TERMINATE_WHEN_FINISHED |
 372				WSM_TX_RATE_POLICY_FLAG_COUNT_INITIAL_TRANSMIT;
 373			memcpy(dst->rate_count_indices, src->tbl,
 374			       sizeof(dst->rate_count_indices));
 375			src->uploaded = 1;
 376			++arg.num;
 377		}
 378	}
 379	spin_unlock_bh(&cache->lock);
 380	cw1200_debug_tx_cache_miss(priv);
 381	pr_debug("[TX policy] Upload %d policies\n", arg.num);
 382	return wsm_set_tx_rate_retry_policy(priv, &arg);
 383}
 384
 385void tx_policy_upload_work(struct work_struct *work)
 386{
 387	struct cw1200_common *priv =
 388		container_of(work, struct cw1200_common, tx_policy_upload_work);
 389
 390	pr_debug("[TX] TX policy upload.\n");
 391	tx_policy_upload(priv);
 392
 393	wsm_unlock_tx(priv);
 394	cw1200_tx_queues_unlock(priv);
 395}
 396
 397/* ******************************************************************** */
 398/* cw1200 TX implementation						*/
 399
 400struct cw1200_txinfo {
 401	struct sk_buff *skb;
 402	unsigned queue;
 403	struct ieee80211_tx_info *tx_info;
 404	const struct ieee80211_rate *rate;
 405	struct ieee80211_hdr *hdr;
 406	size_t hdrlen;
 407	const u8 *da;
 408	struct cw1200_sta_priv *sta_priv;
 409	struct ieee80211_sta *sta;
 410	struct cw1200_txpriv txpriv;
 411};
 412
 413u32 cw1200_rate_mask_to_wsm(struct cw1200_common *priv, u32 rates)
 414{
 415	u32 ret = 0;
 416	int i;
 417	for (i = 0; i < 32; ++i) {
 418		if (rates & BIT(i))
 419			ret |= BIT(priv->rates[i].hw_value);
 420	}
 421	return ret;
 422}
 423
 424static const struct ieee80211_rate *
 425cw1200_get_tx_rate(const struct cw1200_common *priv,
 426		   const struct ieee80211_tx_rate *rate)
 427{
 428	if (rate->idx < 0)
 429		return NULL;
 430	if (rate->flags & IEEE80211_TX_RC_MCS)
 431		return &priv->mcs_rates[rate->idx];
 432	return &priv->hw->wiphy->bands[priv->channel->band]->
 433		bitrates[rate->idx];
 434}
 435
 436static int
 437cw1200_tx_h_calc_link_ids(struct cw1200_common *priv,
 438			  struct cw1200_txinfo *t)
 439{
 440	if (t->sta && t->sta_priv->link_id)
 441		t->txpriv.raw_link_id =
 442				t->txpriv.link_id =
 443				t->sta_priv->link_id;
 444	else if (priv->mode != NL80211_IFTYPE_AP)
 445		t->txpriv.raw_link_id =
 446				t->txpriv.link_id = 0;
 447	else if (is_multicast_ether_addr(t->da)) {
 448		if (priv->enable_beacon) {
 449			t->txpriv.raw_link_id = 0;
 450			t->txpriv.link_id = CW1200_LINK_ID_AFTER_DTIM;
 451		} else {
 452			t->txpriv.raw_link_id = 0;
 453			t->txpriv.link_id = 0;
 454		}
 455	} else {
 456		t->txpriv.link_id = cw1200_find_link_id(priv, t->da);
 457		if (!t->txpriv.link_id)
 458			t->txpriv.link_id = cw1200_alloc_link_id(priv, t->da);
 459		if (!t->txpriv.link_id) {
 460			wiphy_err(priv->hw->wiphy,
 461				  "No more link IDs available.\n");
 462			return -ENOENT;
 463		}
 464		t->txpriv.raw_link_id = t->txpriv.link_id;
 465	}
 466	if (t->txpriv.raw_link_id)
 467		priv->link_id_db[t->txpriv.raw_link_id - 1].timestamp =
 468				jiffies;
 469	if (t->sta && (t->sta->uapsd_queues & BIT(t->queue)))
 470		t->txpriv.link_id = CW1200_LINK_ID_UAPSD;
 471	return 0;
 472}
 473
 474static void
 475cw1200_tx_h_pm(struct cw1200_common *priv,
 476	       struct cw1200_txinfo *t)
 477{
 478	if (ieee80211_is_auth(t->hdr->frame_control)) {
 479		u32 mask = ~BIT(t->txpriv.raw_link_id);
 480		spin_lock_bh(&priv->ps_state_lock);
 481		priv->sta_asleep_mask &= mask;
 482		priv->pspoll_mask &= mask;
 483		spin_unlock_bh(&priv->ps_state_lock);
 484	}
 485}
 486
 487static void
 488cw1200_tx_h_calc_tid(struct cw1200_common *priv,
 489		     struct cw1200_txinfo *t)
 490{
 491	if (ieee80211_is_data_qos(t->hdr->frame_control)) {
 492		u8 *qos = ieee80211_get_qos_ctl(t->hdr);
 493		t->txpriv.tid = qos[0] & IEEE80211_QOS_CTL_TID_MASK;
 494	} else if (ieee80211_is_data(t->hdr->frame_control)) {
 495		t->txpriv.tid = 0;
 496	}
 497}
 498
 499static int
 500cw1200_tx_h_crypt(struct cw1200_common *priv,
 501		  struct cw1200_txinfo *t)
 502{
 503	if (!t->tx_info->control.hw_key ||
 504	    !ieee80211_has_protected(t->hdr->frame_control))
 505		return 0;
 506
 507	t->hdrlen += t->tx_info->control.hw_key->iv_len;
 508	skb_put(t->skb, t->tx_info->control.hw_key->icv_len);
 509
 510	if (t->tx_info->control.hw_key->cipher == WLAN_CIPHER_SUITE_TKIP)
 511		skb_put(t->skb, 8); /* MIC space */
 512
 513	return 0;
 514}
 515
 516static int
 517cw1200_tx_h_align(struct cw1200_common *priv,
 518		  struct cw1200_txinfo *t,
 519		  u8 *flags)
 520{
 521	size_t offset = (size_t)t->skb->data & 3;
 522
 523	if (!offset)
 524		return 0;
 525
 526	if (offset & 1) {
 527		wiphy_err(priv->hw->wiphy,
 528			  "Bug: attempt to transmit a frame with wrong alignment: %zu\n",
 529			  offset);
 530		return -EINVAL;
 531	}
 532
 533	if (skb_headroom(t->skb) < offset) {
 534		wiphy_err(priv->hw->wiphy,
 535			  "Bug: no space allocated for DMA alignment. headroom: %d\n",
 536			  skb_headroom(t->skb));
 537		return -ENOMEM;
 538	}
 539	skb_push(t->skb, offset);
 540	t->hdrlen += offset;
 541	t->txpriv.offset += offset;
 542	*flags |= WSM_TX_2BYTES_SHIFT;
 543	cw1200_debug_tx_align(priv);
 544	return 0;
 545}
 546
 547static int
 548cw1200_tx_h_action(struct cw1200_common *priv,
 549		   struct cw1200_txinfo *t)
 550{
 551	struct ieee80211_mgmt *mgmt =
 552		(struct ieee80211_mgmt *)t->hdr;
 553	if (ieee80211_is_action(t->hdr->frame_control) &&
 554	    mgmt->u.action.category == WLAN_CATEGORY_BACK)
 555		return 1;
 556	else
 557		return 0;
 558}
 559
 560/* Add WSM header */
 561static struct wsm_tx *
 562cw1200_tx_h_wsm(struct cw1200_common *priv,
 563		struct cw1200_txinfo *t)
 564{
 565	struct wsm_tx *wsm;
 566
 567	if (skb_headroom(t->skb) < sizeof(struct wsm_tx)) {
 568		wiphy_err(priv->hw->wiphy,
 569			  "Bug: no space allocated for WSM header. headroom: %d\n",
 570			  skb_headroom(t->skb));
 571		return NULL;
 572	}
 573
 574	wsm = skb_push(t->skb, sizeof(struct wsm_tx));
 575	t->txpriv.offset += sizeof(struct wsm_tx);
 576	memset(wsm, 0, sizeof(*wsm));
 577	wsm->hdr.len = __cpu_to_le16(t->skb->len);
 578	wsm->hdr.id = __cpu_to_le16(0x0004);
 579	wsm->queue_id = wsm_queue_id_to_wsm(t->queue);
 580	return wsm;
 581}
 582
 583/* BT Coex specific handling */
 584static void
 585cw1200_tx_h_bt(struct cw1200_common *priv,
 586	       struct cw1200_txinfo *t,
 587	       struct wsm_tx *wsm)
 588{
 589	u8 priority = 0;
 590
 591	if (!priv->bt_present)
 592		return;
 593
 594	if (ieee80211_is_nullfunc(t->hdr->frame_control)) {
 595		priority = WSM_EPTA_PRIORITY_MGT;
 596	} else if (ieee80211_is_data(t->hdr->frame_control)) {
 597		/* Skip LLC SNAP header (+6) */
 598		u8 *payload = &t->skb->data[t->hdrlen];
 599		__be16 *ethertype = (__be16 *)&payload[6];
 600		if (be16_to_cpu(*ethertype) == ETH_P_PAE)
 601			priority = WSM_EPTA_PRIORITY_EAPOL;
 602	} else if (ieee80211_is_assoc_req(t->hdr->frame_control) ||
 603		ieee80211_is_reassoc_req(t->hdr->frame_control)) {
 604		struct ieee80211_mgmt *mgt_frame =
 605				(struct ieee80211_mgmt *)t->hdr;
 606
 607		if (le16_to_cpu(mgt_frame->u.assoc_req.listen_interval) <
 608						priv->listen_interval) {
 609			pr_debug("Modified Listen Interval to %d from %d\n",
 610				 priv->listen_interval,
 611				 mgt_frame->u.assoc_req.listen_interval);
 612			/* Replace listen interval derieved from
 613			 * the one read from SDD
 614			 */
 615			mgt_frame->u.assoc_req.listen_interval = cpu_to_le16(priv->listen_interval);
 616		}
 617	}
 618
 619	if (!priority) {
 620		if (ieee80211_is_action(t->hdr->frame_control))
 621			priority = WSM_EPTA_PRIORITY_ACTION;
 622		else if (ieee80211_is_mgmt(t->hdr->frame_control))
 623			priority = WSM_EPTA_PRIORITY_MGT;
 624		else if (wsm->queue_id == WSM_QUEUE_VOICE)
 625			priority = WSM_EPTA_PRIORITY_VOICE;
 626		else if (wsm->queue_id == WSM_QUEUE_VIDEO)
 627			priority = WSM_EPTA_PRIORITY_VIDEO;
 628		else
 629			priority = WSM_EPTA_PRIORITY_DATA;
 630	}
 631
 632	pr_debug("[TX] EPTA priority %d.\n", priority);
 633
 634	wsm->flags |= priority << 1;
 635}
 636
 637static int
 638cw1200_tx_h_rate_policy(struct cw1200_common *priv,
 639			struct cw1200_txinfo *t,
 640			struct wsm_tx *wsm)
 641{
 642	bool tx_policy_renew = false;
 643
 644	t->txpriv.rate_id = tx_policy_get(priv,
 645		t->tx_info->control.rates, IEEE80211_TX_MAX_RATES,
 646		&tx_policy_renew);
 647	if (t->txpriv.rate_id == CW1200_INVALID_RATE_ID)
 648		return -EFAULT;
 649
 650	wsm->flags |= t->txpriv.rate_id << 4;
 651
 652	t->rate = cw1200_get_tx_rate(priv,
 653		&t->tx_info->control.rates[0]);
 654	wsm->max_tx_rate = t->rate->hw_value;
 655	if (t->rate->flags & IEEE80211_TX_RC_MCS) {
 656		if (cw1200_ht_greenfield(&priv->ht_info))
 657			wsm->ht_tx_parameters |=
 658				__cpu_to_le32(WSM_HT_TX_GREENFIELD);
 659		else
 660			wsm->ht_tx_parameters |=
 661				__cpu_to_le32(WSM_HT_TX_MIXED);
 662	}
 663
 664	if (tx_policy_renew) {
 665		pr_debug("[TX] TX policy renew.\n");
 666		/* It's not so optimal to stop TX queues every now and then.
 667		 * Better to reimplement task scheduling with
 668		 * a counter. TODO.
 669		 */
 670		wsm_lock_tx_async(priv);
 671		cw1200_tx_queues_lock(priv);
 672		if (queue_work(priv->workqueue,
 673			       &priv->tx_policy_upload_work) <= 0) {
 674			cw1200_tx_queues_unlock(priv);
 675			wsm_unlock_tx(priv);
 676		}
 677	}
 678	return 0;
 679}
 680
 681static bool
 682cw1200_tx_h_pm_state(struct cw1200_common *priv,
 683		     struct cw1200_txinfo *t)
 684{
 685	int was_buffered = 1;
 686
 687	if (t->txpriv.link_id == CW1200_LINK_ID_AFTER_DTIM &&
 688	    !priv->buffered_multicasts) {
 689		priv->buffered_multicasts = true;
 690		if (priv->sta_asleep_mask)
 691			queue_work(priv->workqueue,
 692				   &priv->multicast_start_work);
 693	}
 694
 695	if (t->txpriv.raw_link_id && t->txpriv.tid < CW1200_MAX_TID)
 696		was_buffered = priv->link_id_db[t->txpriv.raw_link_id - 1].buffered[t->txpriv.tid]++;
 697
 698	return !was_buffered;
 699}
 700
 701/* ******************************************************************** */
 702
 703void cw1200_tx(struct ieee80211_hw *dev,
 704	       struct ieee80211_tx_control *control,
 705	       struct sk_buff *skb)
 706{
 707	struct cw1200_common *priv = dev->priv;
 708	struct cw1200_txinfo t = {
 709		.skb = skb,
 710		.queue = skb_get_queue_mapping(skb),
 711		.tx_info = IEEE80211_SKB_CB(skb),
 712		.hdr = (struct ieee80211_hdr *)skb->data,
 713		.txpriv.tid = CW1200_MAX_TID,
 714		.txpriv.rate_id = CW1200_INVALID_RATE_ID,
 715	};
 716	struct ieee80211_sta *sta;
 717	struct wsm_tx *wsm;
 718	bool tid_update = false;
 719	u8 flags = 0;
 720	int ret;
 721
 722	if (priv->bh_error)
 723		goto drop;
 724
 725	t.hdrlen = ieee80211_hdrlen(t.hdr->frame_control);
 726	t.da = ieee80211_get_DA(t.hdr);
 727	if (control) {
 728		t.sta = control->sta;
 729		t.sta_priv = (struct cw1200_sta_priv *)&t.sta->drv_priv;
 730	}
 731
 732	if (WARN_ON(t.queue >= 4))
 733		goto drop;
 734
 735	ret = cw1200_tx_h_calc_link_ids(priv, &t);
 736	if (ret)
 737		goto drop;
 738
 739	pr_debug("[TX] TX %d bytes (queue: %d, link_id: %d (%d)).\n",
 740		 skb->len, t.queue, t.txpriv.link_id,
 741		 t.txpriv.raw_link_id);
 742
 743	cw1200_tx_h_pm(priv, &t);
 744	cw1200_tx_h_calc_tid(priv, &t);
 745	ret = cw1200_tx_h_crypt(priv, &t);
 746	if (ret)
 747		goto drop;
 748	ret = cw1200_tx_h_align(priv, &t, &flags);
 749	if (ret)
 750		goto drop;
 751	ret = cw1200_tx_h_action(priv, &t);
 752	if (ret)
 753		goto drop;
 754	wsm = cw1200_tx_h_wsm(priv, &t);
 755	if (!wsm) {
 756		ret = -ENOMEM;
 757		goto drop;
 758	}
 759	wsm->flags |= flags;
 760	cw1200_tx_h_bt(priv, &t, wsm);
 761	ret = cw1200_tx_h_rate_policy(priv, &t, wsm);
 762	if (ret)
 763		goto drop;
 764
 765	sta = t.sta;
 766
 767	spin_lock_bh(&priv->ps_state_lock);
 768	{
 769		tid_update = cw1200_tx_h_pm_state(priv, &t);
 770		BUG_ON(cw1200_queue_put(&priv->tx_queue[t.queue],
 771					t.skb, &t.txpriv));
 772	}
 773	spin_unlock_bh(&priv->ps_state_lock);
 774
 775	if (tid_update && sta)
 776		ieee80211_sta_set_buffered(sta, t.txpriv.tid, true);
 777
 778	cw1200_bh_wakeup(priv);
 779
 780	return;
 781
 782drop:
 783	cw1200_skb_dtor(priv, skb, &t.txpriv);
 784	return;
 785}
 786
 787/* ******************************************************************** */
 788
 789static int cw1200_handle_action_rx(struct cw1200_common *priv,
 790				   struct sk_buff *skb)
 791{
 792	struct ieee80211_mgmt *mgmt = (void *)skb->data;
 793
 794	/* Filter block ACK negotiation: fully controlled by firmware */
 795	if (mgmt->u.action.category == WLAN_CATEGORY_BACK)
 796		return 1;
 797
 798	return 0;
 799}
 800
 801static int cw1200_handle_pspoll(struct cw1200_common *priv,
 802				struct sk_buff *skb)
 803{
 804	struct ieee80211_sta *sta;
 805	struct ieee80211_pspoll *pspoll = (struct ieee80211_pspoll *)skb->data;
 806	int link_id = 0;
 807	u32 pspoll_mask = 0;
 808	int drop = 1;
 809	int i;
 810
 811	if (priv->join_status != CW1200_JOIN_STATUS_AP)
 812		goto done;
 813	if (memcmp(priv->vif->addr, pspoll->bssid, ETH_ALEN))
 814		goto done;
 815
 816	rcu_read_lock();
 817	sta = ieee80211_find_sta(priv->vif, pspoll->ta);
 818	if (sta) {
 819		struct cw1200_sta_priv *sta_priv;
 820		sta_priv = (struct cw1200_sta_priv *)&sta->drv_priv;
 821		link_id = sta_priv->link_id;
 822		pspoll_mask = BIT(sta_priv->link_id);
 823	}
 824	rcu_read_unlock();
 825	if (!link_id)
 826		goto done;
 827
 828	priv->pspoll_mask |= pspoll_mask;
 829	drop = 0;
 830
 831	/* Do not report pspols if data for given link id is queued already. */
 832	for (i = 0; i < 4; ++i) {
 833		if (cw1200_queue_get_num_queued(&priv->tx_queue[i],
 834						pspoll_mask)) {
 835			cw1200_bh_wakeup(priv);
 836			drop = 1;
 837			break;
 838		}
 839	}
 840	pr_debug("[RX] PSPOLL: %s\n", drop ? "local" : "fwd");
 841done:
 842	return drop;
 843}
 844
 845/* ******************************************************************** */
 846
 847void cw1200_tx_confirm_cb(struct cw1200_common *priv,
 848			  int link_id,
 849			  struct wsm_tx_confirm *arg)
 850{
 851	u8 queue_id = cw1200_queue_get_queue_id(arg->packet_id);
 852	struct cw1200_queue *queue = &priv->tx_queue[queue_id];
 853	struct sk_buff *skb;
 854	const struct cw1200_txpriv *txpriv;
 855
 856	pr_debug("[TX] TX confirm: %d, %d.\n",
 857		 arg->status, arg->ack_failures);
 858
 859	if (priv->mode == NL80211_IFTYPE_UNSPECIFIED) {
 860		/* STA is stopped. */
 861		return;
 862	}
 863
 864	if (WARN_ON(queue_id >= 4))
 865		return;
 866
 867	if (arg->status)
 868		pr_debug("TX failed: %d.\n", arg->status);
 869
 870	if ((arg->status == WSM_REQUEUE) &&
 871	    (arg->flags & WSM_TX_STATUS_REQUEUE)) {
 872		/* "Requeue" means "implicit suspend" */
 873		struct wsm_suspend_resume suspend = {
 874			.link_id = link_id,
 875			.stop = 1,
 876			.multicast = !link_id,
 877		};
 878		cw1200_suspend_resume(priv, &suspend);
 879		wiphy_warn(priv->hw->wiphy, "Requeue for link_id %d (try %d). STAs asleep: 0x%.8X\n",
 880			   link_id,
 881			   cw1200_queue_get_generation(arg->packet_id) + 1,
 882			   priv->sta_asleep_mask);
 883		cw1200_queue_requeue(queue, arg->packet_id);
 884		spin_lock_bh(&priv->ps_state_lock);
 885		if (!link_id) {
 886			priv->buffered_multicasts = true;
 887			if (priv->sta_asleep_mask) {
 888				queue_work(priv->workqueue,
 889					   &priv->multicast_start_work);
 890			}
 891		}
 892		spin_unlock_bh(&priv->ps_state_lock);
 893	} else if (!cw1200_queue_get_skb(queue, arg->packet_id,
 894					 &skb, &txpriv)) {
 895		struct ieee80211_tx_info *tx = IEEE80211_SKB_CB(skb);
 896		int tx_count = arg->ack_failures;
 897		u8 ht_flags = 0;
 898		int i;
 899
 900		if (cw1200_ht_greenfield(&priv->ht_info))
 901			ht_flags |= IEEE80211_TX_RC_GREEN_FIELD;
 902
 903		spin_lock(&priv->bss_loss_lock);
 904		if (priv->bss_loss_state &&
 905		    arg->packet_id == priv->bss_loss_confirm_id) {
 906			if (arg->status) {
 907				/* Recovery failed */
 908				__cw1200_cqm_bssloss_sm(priv, 0, 0, 1);
 909			} else {
 910				/* Recovery succeeded */
 911				__cw1200_cqm_bssloss_sm(priv, 0, 1, 0);
 912			}
 913		}
 914		spin_unlock(&priv->bss_loss_lock);
 915
 916		if (!arg->status) {
 917			tx->flags |= IEEE80211_TX_STAT_ACK;
 918			++tx_count;
 919			cw1200_debug_txed(priv);
 920			if (arg->flags & WSM_TX_STATUS_AGGREGATION) {
 921				/* Do not report aggregation to mac80211:
 922				 * it confuses minstrel a lot.
 923				 */
 924				/* tx->flags |= IEEE80211_TX_STAT_AMPDU; */
 925				cw1200_debug_txed_agg(priv);
 926			}
 927		} else {
 928			if (tx_count)
 929				++tx_count;
 930		}
 931
 932		for (i = 0; i < IEEE80211_TX_MAX_RATES; ++i) {
 933			if (tx->status.rates[i].count >= tx_count) {
 934				tx->status.rates[i].count = tx_count;
 935				break;
 936			}
 937			tx_count -= tx->status.rates[i].count;
 938			if (tx->status.rates[i].flags & IEEE80211_TX_RC_MCS)
 939				tx->status.rates[i].flags |= ht_flags;
 940		}
 941
 942		for (++i; i < IEEE80211_TX_MAX_RATES; ++i) {
 943			tx->status.rates[i].count = 0;
 944			tx->status.rates[i].idx = -1;
 945		}
 946
 947		/* Pull off any crypto trailers that we added on */
 948		if (tx->control.hw_key) {
 949			skb_trim(skb, skb->len - tx->control.hw_key->icv_len);
 950			if (tx->control.hw_key->cipher == WLAN_CIPHER_SUITE_TKIP)
 951				skb_trim(skb, skb->len - 8); /* MIC space */
 952		}
 953		cw1200_queue_remove(queue, arg->packet_id);
 954	}
 955	/* XXX TODO:  Only wake if there are pending transmits.. */
 956	cw1200_bh_wakeup(priv);
 957}
 958
 959static void cw1200_notify_buffered_tx(struct cw1200_common *priv,
 960			       struct sk_buff *skb, int link_id, int tid)
 961{
 962	struct ieee80211_sta *sta;
 963	struct ieee80211_hdr *hdr;
 964	u8 *buffered;
 965	u8 still_buffered = 0;
 966
 967	if (link_id && tid < CW1200_MAX_TID) {
 968		buffered = priv->link_id_db
 969				[link_id - 1].buffered;
 970
 971		spin_lock_bh(&priv->ps_state_lock);
 972		if (!WARN_ON(!buffered[tid]))
 973			still_buffered = --buffered[tid];
 974		spin_unlock_bh(&priv->ps_state_lock);
 975
 976		if (!still_buffered && tid < CW1200_MAX_TID) {
 977			hdr = (struct ieee80211_hdr *)skb->data;
 978			rcu_read_lock();
 979			sta = ieee80211_find_sta(priv->vif, hdr->addr1);
 980			if (sta)
 981				ieee80211_sta_set_buffered(sta, tid, false);
 982			rcu_read_unlock();
 983		}
 984	}
 985}
 986
 987void cw1200_skb_dtor(struct cw1200_common *priv,
 988		     struct sk_buff *skb,
 989		     const struct cw1200_txpriv *txpriv)
 990{
 991	skb_pull(skb, txpriv->offset);
 992	if (txpriv->rate_id != CW1200_INVALID_RATE_ID) {
 993		cw1200_notify_buffered_tx(priv, skb,
 994					  txpriv->raw_link_id, txpriv->tid);
 995		tx_policy_put(priv, txpriv->rate_id);
 996	}
 997	ieee80211_tx_status_skb(priv->hw, skb);
 998}
 999
1000void cw1200_rx_cb(struct cw1200_common *priv,
1001		  struct wsm_rx *arg,
1002		  int link_id,
1003		  struct sk_buff **skb_p)
1004{
1005	struct sk_buff *skb = *skb_p;
1006	struct ieee80211_rx_status *hdr = IEEE80211_SKB_RXCB(skb);
1007	struct ieee80211_hdr *frame = (struct ieee80211_hdr *)skb->data;
1008	struct ieee80211_mgmt *mgmt = (struct ieee80211_mgmt *)skb->data;
1009	struct cw1200_link_entry *entry = NULL;
1010	unsigned long grace_period;
1011
1012	bool early_data = false;
1013	bool p2p = priv->vif && priv->vif->p2p;
1014	size_t hdrlen;
1015	hdr->flag = 0;
1016
1017	if (priv->mode == NL80211_IFTYPE_UNSPECIFIED) {
1018		/* STA is stopped. */
1019		goto drop;
1020	}
1021
1022	if (link_id && link_id <= CW1200_MAX_STA_IN_AP_MODE) {
1023		entry =	&priv->link_id_db[link_id - 1];
1024		if (entry->status == CW1200_LINK_SOFT &&
1025		    ieee80211_is_data(frame->frame_control))
1026			early_data = true;
1027		entry->timestamp = jiffies;
1028	} else if (p2p &&
1029		   ieee80211_is_action(frame->frame_control) &&
1030		   (mgmt->u.action.category == WLAN_CATEGORY_PUBLIC)) {
1031		pr_debug("[RX] Going to MAP&RESET link ID\n");
1032		WARN_ON(work_pending(&priv->linkid_reset_work));
1033		memcpy(&priv->action_frame_sa[0],
1034		       ieee80211_get_SA(frame), ETH_ALEN);
1035		priv->action_linkid = 0;
1036		schedule_work(&priv->linkid_reset_work);
1037	}
1038
1039	if (link_id && p2p &&
1040	    ieee80211_is_action(frame->frame_control) &&
1041	    (mgmt->u.action.category == WLAN_CATEGORY_PUBLIC)) {
1042		/* Link ID already exists for the ACTION frame.
1043		 * Reset and Remap
1044		 */
1045		WARN_ON(work_pending(&priv->linkid_reset_work));
1046		memcpy(&priv->action_frame_sa[0],
1047		       ieee80211_get_SA(frame), ETH_ALEN);
1048		priv->action_linkid = link_id;
1049		schedule_work(&priv->linkid_reset_work);
1050	}
1051	if (arg->status) {
1052		if (arg->status == WSM_STATUS_MICFAILURE) {
1053			pr_debug("[RX] MIC failure.\n");
1054			hdr->flag |= RX_FLAG_MMIC_ERROR;
1055		} else if (arg->status == WSM_STATUS_NO_KEY_FOUND) {
1056			pr_debug("[RX] No key found.\n");
1057			goto drop;
1058		} else {
1059			pr_debug("[RX] Receive failure: %d.\n",
1060				 arg->status);
1061			goto drop;
1062		}
1063	}
1064
1065	if (skb->len < sizeof(struct ieee80211_pspoll)) {
1066		wiphy_warn(priv->hw->wiphy, "Malformed SDU rx'ed. Size is lesser than IEEE header.\n");
1067		goto drop;
1068	}
1069
1070	if (ieee80211_is_pspoll(frame->frame_control))
1071		if (cw1200_handle_pspoll(priv, skb))
1072			goto drop;
1073
1074	hdr->band = ((arg->channel_number & 0xff00) ||
1075		     (arg->channel_number > 14)) ?
1076			NL80211_BAND_5GHZ : NL80211_BAND_2GHZ;
1077	hdr->freq = ieee80211_channel_to_frequency(
1078			arg->channel_number,
1079			hdr->band);
1080
1081	if (arg->rx_rate >= 14) {
1082		hdr->encoding = RX_ENC_HT;
1083		hdr->rate_idx = arg->rx_rate - 14;
1084	} else if (arg->rx_rate >= 4) {
1085		hdr->rate_idx = arg->rx_rate - 2;
1086	} else {
1087		hdr->rate_idx = arg->rx_rate;
1088	}
1089
1090	hdr->signal = (s8)arg->rcpi_rssi;
1091	hdr->antenna = 0;
1092
1093	hdrlen = ieee80211_hdrlen(frame->frame_control);
1094
1095	if (WSM_RX_STATUS_ENCRYPTION(arg->flags)) {
1096		size_t iv_len = 0, icv_len = 0;
1097
1098		hdr->flag |= RX_FLAG_DECRYPTED | RX_FLAG_IV_STRIPPED;
1099
1100		/* Oops... There is no fast way to ask mac80211 about
1101		 * IV/ICV lengths. Even defineas are not exposed.
1102		 */
1103		switch (WSM_RX_STATUS_ENCRYPTION(arg->flags)) {
1104		case WSM_RX_STATUS_WEP:
1105			iv_len = 4 /* WEP_IV_LEN */;
1106			icv_len = 4 /* WEP_ICV_LEN */;
1107			break;
1108		case WSM_RX_STATUS_TKIP:
1109			iv_len = 8 /* TKIP_IV_LEN */;
1110			icv_len = 4 /* TKIP_ICV_LEN */
1111				+ 8 /*MICHAEL_MIC_LEN*/;
1112			hdr->flag |= RX_FLAG_MMIC_STRIPPED;
1113			break;
1114		case WSM_RX_STATUS_AES:
1115			iv_len = 8 /* CCMP_HDR_LEN */;
1116			icv_len = 8 /* CCMP_MIC_LEN */;
1117			break;
1118		case WSM_RX_STATUS_WAPI:
1119			iv_len = 18 /* WAPI_HDR_LEN */;
1120			icv_len = 16 /* WAPI_MIC_LEN */;
1121			break;
1122		default:
1123			pr_warn("Unknown encryption type %d\n",
1124				WSM_RX_STATUS_ENCRYPTION(arg->flags));
1125			goto drop;
1126		}
1127
1128		/* Firmware strips ICV in case of MIC failure. */
1129		if (arg->status == WSM_STATUS_MICFAILURE)
1130			icv_len = 0;
1131
1132		if (skb->len < hdrlen + iv_len + icv_len) {
1133			wiphy_warn(priv->hw->wiphy, "Malformed SDU rx'ed. Size is lesser than crypto headers.\n");
1134			goto drop;
1135		}
1136
1137		/* Remove IV, ICV and MIC */
1138		skb_trim(skb, skb->len - icv_len);
1139		memmove(skb->data + iv_len, skb->data, hdrlen);
1140		skb_pull(skb, iv_len);
1141	}
1142
1143	/* Remove TSF from the end of frame */
1144	if (arg->flags & WSM_RX_STATUS_TSF_INCLUDED) {
1145		hdr->mactime = get_unaligned_le64(skb->data + skb->len - 8);
1146		if (skb->len >= 8)
1147			skb_trim(skb, skb->len - 8);
1148	} else {
1149		hdr->mactime = 0;
1150	}
1151
1152	cw1200_debug_rxed(priv);
1153	if (arg->flags & WSM_RX_STATUS_AGGREGATE)
1154		cw1200_debug_rxed_agg(priv);
1155
1156	if (ieee80211_is_action(frame->frame_control) &&
1157	    (arg->flags & WSM_RX_STATUS_ADDRESS1)) {
1158		if (cw1200_handle_action_rx(priv, skb))
1159			return;
1160	} else if (ieee80211_is_beacon(frame->frame_control) &&
1161		   !arg->status && priv->vif &&
1162		   ether_addr_equal(ieee80211_get_SA(frame), priv->vif->bss_conf.bssid)) {
1163		const u8 *tim_ie;
1164		u8 *ies = ((struct ieee80211_mgmt *)
1165			  (skb->data))->u.beacon.variable;
1166		size_t ies_len = skb->len - (ies - (u8 *)(skb->data));
1167
1168		tim_ie = cfg80211_find_ie(WLAN_EID_TIM, ies, ies_len);
1169		if (tim_ie && tim_ie[1] >= sizeof(struct ieee80211_tim_ie)) {
1170			struct ieee80211_tim_ie *tim =
1171				(struct ieee80211_tim_ie *)&tim_ie[2];
1172
1173			if (priv->join_dtim_period != tim->dtim_period) {
1174				priv->join_dtim_period = tim->dtim_period;
1175				queue_work(priv->workqueue,
1176					   &priv->set_beacon_wakeup_period_work);
1177			}
1178		}
1179
1180		/* Disable beacon filter once we're associated... */
1181		if (priv->disable_beacon_filter &&
1182		    (priv->vif->cfg.assoc ||
1183		     priv->vif->cfg.ibss_joined)) {
1184			priv->disable_beacon_filter = false;
1185			queue_work(priv->workqueue,
1186				   &priv->update_filtering_work);
1187		}
1188	}
1189
1190	/* Stay awake after frame is received to give
1191	 * userspace chance to react and acquire appropriate
1192	 * wakelock.
1193	 */
1194	if (ieee80211_is_auth(frame->frame_control))
1195		grace_period = 5 * HZ;
1196	else if (ieee80211_is_deauth(frame->frame_control))
1197		grace_period = 5 * HZ;
1198	else
1199		grace_period = 1 * HZ;
1200	cw1200_pm_stay_awake(&priv->pm_state, grace_period);
1201
1202	if (early_data) {
1203		spin_lock_bh(&priv->ps_state_lock);
1204		/* Double-check status with lock held */
1205		if (entry->status == CW1200_LINK_SOFT)
1206			skb_queue_tail(&entry->rx_queue, skb);
1207		else
1208			ieee80211_rx_irqsafe(priv->hw, skb);
1209		spin_unlock_bh(&priv->ps_state_lock);
1210	} else {
1211		ieee80211_rx_irqsafe(priv->hw, skb);
1212	}
1213	*skb_p = NULL;
1214
1215	return;
1216
1217drop:
1218	/* TODO: update failure counters */
1219	return;
1220}
1221
1222/* ******************************************************************** */
1223/* Security								*/
1224
1225int cw1200_alloc_key(struct cw1200_common *priv)
1226{
1227	int idx;
1228
1229	idx = ffs(~priv->key_map) - 1;
1230	if (idx < 0 || idx > WSM_KEY_MAX_INDEX)
1231		return -1;
1232
1233	priv->key_map |= BIT(idx);
1234	priv->keys[idx].index = idx;
1235	return idx;
1236}
1237
1238void cw1200_free_key(struct cw1200_common *priv, int idx)
1239{
1240	BUG_ON(!(priv->key_map & BIT(idx)));
1241	memset(&priv->keys[idx], 0, sizeof(priv->keys[idx]));
1242	priv->key_map &= ~BIT(idx);
1243}
1244
1245void cw1200_free_keys(struct cw1200_common *priv)
1246{
1247	memset(&priv->keys, 0, sizeof(priv->keys));
1248	priv->key_map = 0;
1249}
1250
1251int cw1200_upload_keys(struct cw1200_common *priv)
1252{
1253	int idx, ret = 0;
1254	for (idx = 0; idx <= WSM_KEY_MAX_INDEX; ++idx)
1255		if (priv->key_map & BIT(idx)) {
1256			ret = wsm_add_key(priv, &priv->keys[idx]);
1257			if (ret < 0)
1258				break;
1259		}
1260	return ret;
1261}
1262
1263/* Workaround for WFD test case 6.1.10 */
1264void cw1200_link_id_reset(struct work_struct *work)
1265{
1266	struct cw1200_common *priv =
1267		container_of(work, struct cw1200_common, linkid_reset_work);
1268	int temp_linkid;
1269
1270	if (!priv->action_linkid) {
1271		/* In GO mode we can receive ACTION frames without a linkID */
1272		temp_linkid = cw1200_alloc_link_id(priv,
1273				&priv->action_frame_sa[0]);
1274		WARN_ON(!temp_linkid);
1275		if (temp_linkid) {
1276			/* Make sure we execute the WQ */
1277			flush_workqueue(priv->workqueue);
1278			/* Release the link ID */
1279			spin_lock_bh(&priv->ps_state_lock);
1280			priv->link_id_db[temp_linkid - 1].prev_status =
1281				priv->link_id_db[temp_linkid - 1].status;
1282			priv->link_id_db[temp_linkid - 1].status =
1283				CW1200_LINK_RESET;
1284			spin_unlock_bh(&priv->ps_state_lock);
1285			wsm_lock_tx_async(priv);
1286			if (queue_work(priv->workqueue,
1287				       &priv->link_id_work) <= 0)
1288				wsm_unlock_tx(priv);
1289		}
1290	} else {
1291		spin_lock_bh(&priv->ps_state_lock);
1292		priv->link_id_db[priv->action_linkid - 1].prev_status =
1293			priv->link_id_db[priv->action_linkid - 1].status;
1294		priv->link_id_db[priv->action_linkid - 1].status =
1295			CW1200_LINK_RESET_REMAP;
1296		spin_unlock_bh(&priv->ps_state_lock);
1297		wsm_lock_tx_async(priv);
1298		if (queue_work(priv->workqueue, &priv->link_id_work) <= 0)
1299			wsm_unlock_tx(priv);
1300		flush_workqueue(priv->workqueue);
1301	}
1302}
1303
1304int cw1200_find_link_id(struct cw1200_common *priv, const u8 *mac)
1305{
1306	int i, ret = 0;
1307	spin_lock_bh(&priv->ps_state_lock);
1308	for (i = 0; i < CW1200_MAX_STA_IN_AP_MODE; ++i) {
1309		if (!memcmp(mac, priv->link_id_db[i].mac, ETH_ALEN) &&
1310		    priv->link_id_db[i].status) {
1311			priv->link_id_db[i].timestamp = jiffies;
1312			ret = i + 1;
1313			break;
1314		}
1315	}
1316	spin_unlock_bh(&priv->ps_state_lock);
1317	return ret;
1318}
1319
1320int cw1200_alloc_link_id(struct cw1200_common *priv, const u8 *mac)
1321{
1322	int i, ret = 0;
1323	unsigned long max_inactivity = 0;
1324	unsigned long now = jiffies;
1325
1326	spin_lock_bh(&priv->ps_state_lock);
1327	for (i = 0; i < CW1200_MAX_STA_IN_AP_MODE; ++i) {
1328		if (!priv->link_id_db[i].status) {
1329			ret = i + 1;
1330			break;
1331		} else if (priv->link_id_db[i].status != CW1200_LINK_HARD &&
1332			   !priv->tx_queue_stats.link_map_cache[i + 1]) {
1333			unsigned long inactivity =
1334				now - priv->link_id_db[i].timestamp;
1335			if (inactivity < max_inactivity)
1336				continue;
1337			max_inactivity = inactivity;
1338			ret = i + 1;
1339		}
1340	}
1341	if (ret) {
1342		struct cw1200_link_entry *entry = &priv->link_id_db[ret - 1];
1343		pr_debug("[AP] STA added, link_id: %d\n", ret);
1344		entry->status = CW1200_LINK_RESERVE;
1345		memcpy(&entry->mac, mac, ETH_ALEN);
1346		memset(&entry->buffered, 0, CW1200_MAX_TID);
1347		skb_queue_head_init(&entry->rx_queue);
1348		wsm_lock_tx_async(priv);
1349		if (queue_work(priv->workqueue, &priv->link_id_work) <= 0)
1350			wsm_unlock_tx(priv);
1351	} else {
1352		wiphy_info(priv->hw->wiphy,
1353			   "[AP] Early: no more link IDs available.\n");
1354	}
1355
1356	spin_unlock_bh(&priv->ps_state_lock);
1357	return ret;
1358}
1359
1360void cw1200_link_id_work(struct work_struct *work)
1361{
1362	struct cw1200_common *priv =
1363		container_of(work, struct cw1200_common, link_id_work);
1364	wsm_flush_tx(priv);
1365	cw1200_link_id_gc_work(&priv->link_id_gc_work.work);
1366	wsm_unlock_tx(priv);
1367}
1368
1369void cw1200_link_id_gc_work(struct work_struct *work)
1370{
1371	struct cw1200_common *priv =
1372		container_of(work, struct cw1200_common, link_id_gc_work.work);
1373	struct wsm_reset reset = {
1374		.reset_statistics = false,
1375	};
1376	struct wsm_map_link map_link = {
1377		.link_id = 0,
1378	};
1379	unsigned long now = jiffies;
1380	unsigned long next_gc = -1;
1381	long ttl;
1382	bool need_reset;
1383	u32 mask;
1384	int i;
1385
1386	if (priv->join_status != CW1200_JOIN_STATUS_AP)
1387		return;
1388
1389	wsm_lock_tx(priv);
1390	spin_lock_bh(&priv->ps_state_lock);
1391	for (i = 0; i < CW1200_MAX_STA_IN_AP_MODE; ++i) {
1392		need_reset = false;
1393		mask = BIT(i + 1);
1394		if (priv->link_id_db[i].status == CW1200_LINK_RESERVE ||
1395		    (priv->link_id_db[i].status == CW1200_LINK_HARD &&
1396		     !(priv->link_id_map & mask))) {
1397			if (priv->link_id_map & mask) {
1398				priv->sta_asleep_mask &= ~mask;
1399				priv->pspoll_mask &= ~mask;
1400				need_reset = true;
1401			}
1402			priv->link_id_map |= mask;
1403			if (priv->link_id_db[i].status != CW1200_LINK_HARD)
1404				priv->link_id_db[i].status = CW1200_LINK_SOFT;
1405			memcpy(map_link.mac_addr, priv->link_id_db[i].mac,
1406			       ETH_ALEN);
1407			spin_unlock_bh(&priv->ps_state_lock);
1408			if (need_reset) {
1409				reset.link_id = i + 1;
1410				wsm_reset(priv, &reset);
1411			}
1412			map_link.link_id = i + 1;
1413			wsm_map_link(priv, &map_link);
1414			next_gc = min(next_gc, CW1200_LINK_ID_GC_TIMEOUT);
1415			spin_lock_bh(&priv->ps_state_lock);
1416		} else if (priv->link_id_db[i].status == CW1200_LINK_SOFT) {
1417			ttl = priv->link_id_db[i].timestamp - now +
1418					CW1200_LINK_ID_GC_TIMEOUT;
1419			if (ttl <= 0) {
1420				need_reset = true;
1421				priv->link_id_db[i].status = CW1200_LINK_OFF;
1422				priv->link_id_map &= ~mask;
1423				priv->sta_asleep_mask &= ~mask;
1424				priv->pspoll_mask &= ~mask;
1425				eth_zero_addr(map_link.mac_addr);
1426				spin_unlock_bh(&priv->ps_state_lock);
1427				reset.link_id = i + 1;
1428				wsm_reset(priv, &reset);
1429				spin_lock_bh(&priv->ps_state_lock);
1430			} else {
1431				next_gc = min_t(unsigned long, next_gc, ttl);
1432			}
1433		} else if (priv->link_id_db[i].status == CW1200_LINK_RESET ||
1434				priv->link_id_db[i].status ==
1435				CW1200_LINK_RESET_REMAP) {
1436			int status = priv->link_id_db[i].status;
1437			priv->link_id_db[i].status =
1438					priv->link_id_db[i].prev_status;
1439			priv->link_id_db[i].timestamp = now;
1440			reset.link_id = i + 1;
1441			spin_unlock_bh(&priv->ps_state_lock);
1442			wsm_reset(priv, &reset);
1443			if (status == CW1200_LINK_RESET_REMAP) {
1444				memcpy(map_link.mac_addr,
1445				       priv->link_id_db[i].mac,
1446				       ETH_ALEN);
1447				map_link.link_id = i + 1;
1448				wsm_map_link(priv, &map_link);
1449				next_gc = min(next_gc,
1450						CW1200_LINK_ID_GC_TIMEOUT);
1451			}
1452			spin_lock_bh(&priv->ps_state_lock);
1453		}
1454		if (need_reset) {
1455			skb_queue_purge(&priv->link_id_db[i].rx_queue);
1456			pr_debug("[AP] STA removed, link_id: %d\n",
1457				 reset.link_id);
1458		}
1459	}
1460	spin_unlock_bh(&priv->ps_state_lock);
1461	if (next_gc != -1)
1462		queue_delayed_work(priv->workqueue,
1463				   &priv->link_id_gc_work, next_gc);
1464	wsm_unlock_tx(priv);
1465}