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 21 static int cw1200_handle_action_rx(struct cw1200_common *priv, 22 struct sk_buff *skb); 23 static const struct ieee80211_rate * 24 cw1200_get_tx_rate(const struct cw1200_common *priv, 25 const struct ieee80211_tx_rate *rate); 26 27 /* ******************************************************************** */ 28 /* TX queue lock / unlock */ 29 30 static 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 37 static 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 47 static 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 65 static 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 196 static 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 213 static 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 234 static 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 241 static 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 250 void 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 281 void 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 296 static 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 337 static 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 352 static 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 385 void 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 400 struct 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 413 u32 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 424 static const struct ieee80211_rate * 425 cw1200_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 436 static int 437 cw1200_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 474 static void 475 cw1200_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 487 static void 488 cw1200_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 499 static int 500 cw1200_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 516 static int 517 cw1200_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 547 static int 548 cw1200_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 */ 561 static struct wsm_tx * 562 cw1200_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 */ 584 static void 585 cw1200_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 637 static int 638 cw1200_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 681 static bool 682 cw1200_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 703 void 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 782 drop: 783 cw1200_skb_dtor(priv, skb, &t.txpriv); 784 return; 785 } 786 787 /* ******************************************************************** */ 788 789 static 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 801 static 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"); 841 done: 842 return drop; 843 } 844 845 /* ******************************************************************** */ 846 847 void 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 959 static 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 987 void 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(priv->hw, skb); 998 } 999 1000 void 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) { 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 1217 drop: 1218 /* TODO: update failure counters */ 1219 return; 1220 } 1221 1222 /* ******************************************************************** */ 1223 /* Security */ 1224 1225 int 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 1238 void 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 1245 void cw1200_free_keys(struct cw1200_common *priv) 1246 { 1247 memset(&priv->keys, 0, sizeof(priv->keys)); 1248 priv->key_map = 0; 1249 } 1250 1251 int 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 */ 1264 void 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 1304 int 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 1320 int 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 1360 void 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 1369 void 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 } 1466