drivers/net/wireless/cw1200/txrx.c

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