drivers/net/wireless/st/cw1200/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
  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_skb(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 && 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
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 }