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