Back to home page

OSCL-LXR
 
 

     

0001 // SPDX-License-Identifier: GPL-2.0-only
0002 /*
0003  * Datapath implementation for ST-Ericsson CW1200 mac80211 drivers
0004  *
0005  * Copyright (c) 2010, ST-Ericsson
0006  * Author: Dmitry Tarnyagin <dmitry.tarnyagin@lockless.no>
0007  */
0008 
0009 #include <net/mac80211.h>
0010 #include <linux/etherdevice.h>
0011 #include <linux/skbuff.h>
0012 
0013 #include "cw1200.h"
0014 #include "wsm.h"
0015 #include "bh.h"
0016 #include "sta.h"
0017 #include "debug.h"
0018 
0019 #define CW1200_INVALID_RATE_ID (0xFF)
0020 
0021 static int cw1200_handle_action_rx(struct cw1200_common *priv,
0022                    struct sk_buff *skb);
0023 static const struct ieee80211_rate *
0024 cw1200_get_tx_rate(const struct cw1200_common *priv,
0025            const struct ieee80211_tx_rate *rate);
0026 
0027 /* ******************************************************************** */
0028 /* TX queue lock / unlock                       */
0029 
0030 static inline void cw1200_tx_queues_lock(struct cw1200_common *priv)
0031 {
0032     int i;
0033     for (i = 0; i < 4; ++i)
0034         cw1200_queue_lock(&priv->tx_queue[i]);
0035 }
0036 
0037 static inline void cw1200_tx_queues_unlock(struct cw1200_common *priv)
0038 {
0039     int i;
0040     for (i = 0; i < 4; ++i)
0041         cw1200_queue_unlock(&priv->tx_queue[i]);
0042 }
0043 
0044 /* ******************************************************************** */
0045 /* TX policy cache implementation                   */
0046 
0047 static void tx_policy_dump(struct tx_policy *policy)
0048 {
0049     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",
0050          policy->raw[0] & 0x0F,  policy->raw[0] >> 4,
0051          policy->raw[1] & 0x0F,  policy->raw[1] >> 4,
0052          policy->raw[2] & 0x0F,  policy->raw[2] >> 4,
0053          policy->raw[3] & 0x0F,  policy->raw[3] >> 4,
0054          policy->raw[4] & 0x0F,  policy->raw[4] >> 4,
0055          policy->raw[5] & 0x0F,  policy->raw[5] >> 4,
0056          policy->raw[6] & 0x0F,  policy->raw[6] >> 4,
0057          policy->raw[7] & 0x0F,  policy->raw[7] >> 4,
0058          policy->raw[8] & 0x0F,  policy->raw[8] >> 4,
0059          policy->raw[9] & 0x0F,  policy->raw[9] >> 4,
0060          policy->raw[10] & 0x0F,  policy->raw[10] >> 4,
0061          policy->raw[11] & 0x0F,  policy->raw[11] >> 4,
0062          policy->defined);
0063 }
0064 
0065 static void tx_policy_build(const struct cw1200_common *priv,
0066     /* [out] */ struct tx_policy *policy,
0067     struct ieee80211_tx_rate *rates, size_t count)
0068 {
0069     int i, j;
0070     unsigned limit = priv->short_frame_max_tx_count;
0071     unsigned total = 0;
0072     BUG_ON(rates[0].idx < 0);
0073     memset(policy, 0, sizeof(*policy));
0074 
0075     /* Sort rates in descending order. */
0076     for (i = 1; i < count; ++i) {
0077         if (rates[i].idx < 0) {
0078             count = i;
0079             break;
0080         }
0081         if (rates[i].idx > rates[i - 1].idx) {
0082             struct ieee80211_tx_rate tmp = rates[i - 1];
0083             rates[i - 1] = rates[i];
0084             rates[i] = tmp;
0085         }
0086     }
0087 
0088     /* Eliminate duplicates. */
0089     total = rates[0].count;
0090     for (i = 0, j = 1; j < count; ++j) {
0091         if (rates[j].idx == rates[i].idx) {
0092             rates[i].count += rates[j].count;
0093         } else if (rates[j].idx > rates[i].idx) {
0094             break;
0095         } else {
0096             ++i;
0097             if (i != j)
0098                 rates[i] = rates[j];
0099         }
0100         total += rates[j].count;
0101     }
0102     count = i + 1;
0103 
0104     /* Re-fill policy trying to keep every requested rate and with
0105      * respect to the global max tx retransmission count.
0106      */
0107     if (limit < count)
0108         limit = count;
0109     if (total > limit) {
0110         for (i = 0; i < count; ++i) {
0111             int left = count - i - 1;
0112             if (rates[i].count > limit - left)
0113                 rates[i].count = limit - left;
0114             limit -= rates[i].count;
0115         }
0116     }
0117 
0118     /* HACK!!! Device has problems (at least) switching from
0119      * 54Mbps CTS to 1Mbps. This switch takes enormous amount
0120      * of time (100-200 ms), leading to valuable throughput drop.
0121      * As a workaround, additional g-rates are injected to the
0122      * policy.
0123      */
0124     if (count == 2 && !(rates[0].flags & IEEE80211_TX_RC_MCS) &&
0125         rates[0].idx > 4 && rates[0].count > 2 &&
0126         rates[1].idx < 2) {
0127         int mid_rate = (rates[0].idx + 4) >> 1;
0128 
0129         /* Decrease number of retries for the initial rate */
0130         rates[0].count -= 2;
0131 
0132         if (mid_rate != 4) {
0133             /* Keep fallback rate at 1Mbps. */
0134             rates[3] = rates[1];
0135 
0136             /* Inject 1 transmission on lowest g-rate */
0137             rates[2].idx = 4;
0138             rates[2].count = 1;
0139             rates[2].flags = rates[1].flags;
0140 
0141             /* Inject 1 transmission on mid-rate */
0142             rates[1].idx = mid_rate;
0143             rates[1].count = 1;
0144 
0145             /* Fallback to 1 Mbps is a really bad thing,
0146              * so let's try to increase probability of
0147              * successful transmission on the lowest g rate
0148              * even more
0149              */
0150             if (rates[0].count >= 3) {
0151                 --rates[0].count;
0152                 ++rates[2].count;
0153             }
0154 
0155             /* Adjust amount of rates defined */
0156             count += 2;
0157         } else {
0158             /* Keep fallback rate at 1Mbps. */
0159             rates[2] = rates[1];
0160 
0161             /* Inject 2 transmissions on lowest g-rate */
0162             rates[1].idx = 4;
0163             rates[1].count = 2;
0164 
0165             /* Adjust amount of rates defined */
0166             count += 1;
0167         }
0168     }
0169 
0170     policy->defined = cw1200_get_tx_rate(priv, &rates[0])->hw_value + 1;
0171 
0172     for (i = 0; i < count; ++i) {
0173         register unsigned rateid, off, shift, retries;
0174 
0175         rateid = cw1200_get_tx_rate(priv, &rates[i])->hw_value;
0176         off = rateid >> 3;      /* eq. rateid / 8 */
0177         shift = (rateid & 0x07) << 2;   /* eq. (rateid % 8) * 4 */
0178 
0179         retries = rates[i].count;
0180         if (retries > 0x0F) {
0181             rates[i].count = 0x0f;
0182             retries = 0x0F;
0183         }
0184         policy->tbl[off] |= __cpu_to_le32(retries << shift);
0185         policy->retry_count += retries;
0186     }
0187 
0188     pr_debug("[TX policy] Policy (%zu): %d:%d, %d:%d, %d:%d, %d:%d\n",
0189          count,
0190          rates[0].idx, rates[0].count,
0191          rates[1].idx, rates[1].count,
0192          rates[2].idx, rates[2].count,
0193          rates[3].idx, rates[3].count);
0194 }
0195 
0196 static inline bool tx_policy_is_equal(const struct tx_policy *wanted,
0197                     const struct tx_policy *cached)
0198 {
0199     size_t count = wanted->defined >> 1;
0200     if (wanted->defined > cached->defined)
0201         return false;
0202     if (count) {
0203         if (memcmp(wanted->raw, cached->raw, count))
0204             return false;
0205     }
0206     if (wanted->defined & 1) {
0207         if ((wanted->raw[count] & 0x0F) != (cached->raw[count] & 0x0F))
0208             return false;
0209     }
0210     return true;
0211 }
0212 
0213 static int tx_policy_find(struct tx_policy_cache *cache,
0214                 const struct tx_policy *wanted)
0215 {
0216     /* O(n) complexity. Not so good, but there's only 8 entries in
0217      * the cache.
0218      * Also lru helps to reduce search time.
0219      */
0220     struct tx_policy_cache_entry *it;
0221     /* First search for policy in "used" list */
0222     list_for_each_entry(it, &cache->used, link) {
0223         if (tx_policy_is_equal(wanted, &it->policy))
0224             return it - cache->cache;
0225     }
0226     /* Then - in "free list" */
0227     list_for_each_entry(it, &cache->free, link) {
0228         if (tx_policy_is_equal(wanted, &it->policy))
0229             return it - cache->cache;
0230     }
0231     return -1;
0232 }
0233 
0234 static inline void tx_policy_use(struct tx_policy_cache *cache,
0235                  struct tx_policy_cache_entry *entry)
0236 {
0237     ++entry->policy.usage_count;
0238     list_move(&entry->link, &cache->used);
0239 }
0240 
0241 static inline int tx_policy_release(struct tx_policy_cache *cache,
0242                     struct tx_policy_cache_entry *entry)
0243 {
0244     int ret = --entry->policy.usage_count;
0245     if (!ret)
0246         list_move(&entry->link, &cache->free);
0247     return ret;
0248 }
0249 
0250 void tx_policy_clean(struct cw1200_common *priv)
0251 {
0252     int idx, locked;
0253     struct tx_policy_cache *cache = &priv->tx_policy_cache;
0254     struct tx_policy_cache_entry *entry;
0255 
0256     cw1200_tx_queues_lock(priv);
0257     spin_lock_bh(&cache->lock);
0258     locked = list_empty(&cache->free);
0259 
0260     for (idx = 0; idx < TX_POLICY_CACHE_SIZE; idx++) {
0261         entry = &cache->cache[idx];
0262         /* Policy usage count should be 0 at this time as all queues
0263            should be empty
0264          */
0265         if (WARN_ON(entry->policy.usage_count)) {
0266             entry->policy.usage_count = 0;
0267             list_move(&entry->link, &cache->free);
0268         }
0269         memset(&entry->policy, 0, sizeof(entry->policy));
0270     }
0271     if (locked)
0272         cw1200_tx_queues_unlock(priv);
0273 
0274     cw1200_tx_queues_unlock(priv);
0275     spin_unlock_bh(&cache->lock);
0276 }
0277 
0278 /* ******************************************************************** */
0279 /* External TX policy cache API                     */
0280 
0281 void tx_policy_init(struct cw1200_common *priv)
0282 {
0283     struct tx_policy_cache *cache = &priv->tx_policy_cache;
0284     int i;
0285 
0286     memset(cache, 0, sizeof(*cache));
0287 
0288     spin_lock_init(&cache->lock);
0289     INIT_LIST_HEAD(&cache->used);
0290     INIT_LIST_HEAD(&cache->free);
0291 
0292     for (i = 0; i < TX_POLICY_CACHE_SIZE; ++i)
0293         list_add(&cache->cache[i].link, &cache->free);
0294 }
0295 
0296 static int tx_policy_get(struct cw1200_common *priv,
0297           struct ieee80211_tx_rate *rates,
0298           size_t count, bool *renew)
0299 {
0300     int idx;
0301     struct tx_policy_cache *cache = &priv->tx_policy_cache;
0302     struct tx_policy wanted;
0303 
0304     tx_policy_build(priv, &wanted, rates, count);
0305 
0306     spin_lock_bh(&cache->lock);
0307     if (WARN_ON_ONCE(list_empty(&cache->free))) {
0308         spin_unlock_bh(&cache->lock);
0309         return CW1200_INVALID_RATE_ID;
0310     }
0311     idx = tx_policy_find(cache, &wanted);
0312     if (idx >= 0) {
0313         pr_debug("[TX policy] Used TX policy: %d\n", idx);
0314         *renew = false;
0315     } else {
0316         struct tx_policy_cache_entry *entry;
0317         *renew = true;
0318         /* If policy is not found create a new one
0319          * using the oldest entry in "free" list
0320          */
0321         entry = list_entry(cache->free.prev,
0322             struct tx_policy_cache_entry, link);
0323         entry->policy = wanted;
0324         idx = entry - cache->cache;
0325         pr_debug("[TX policy] New TX policy: %d\n", idx);
0326         tx_policy_dump(&entry->policy);
0327     }
0328     tx_policy_use(cache, &cache->cache[idx]);
0329     if (list_empty(&cache->free)) {
0330         /* Lock TX queues. */
0331         cw1200_tx_queues_lock(priv);
0332     }
0333     spin_unlock_bh(&cache->lock);
0334     return idx;
0335 }
0336 
0337 static void tx_policy_put(struct cw1200_common *priv, int idx)
0338 {
0339     int usage, locked;
0340     struct tx_policy_cache *cache = &priv->tx_policy_cache;
0341 
0342     spin_lock_bh(&cache->lock);
0343     locked = list_empty(&cache->free);
0344     usage = tx_policy_release(cache, &cache->cache[idx]);
0345     if (locked && !usage) {
0346         /* Unlock TX queues. */
0347         cw1200_tx_queues_unlock(priv);
0348     }
0349     spin_unlock_bh(&cache->lock);
0350 }
0351 
0352 static int tx_policy_upload(struct cw1200_common *priv)
0353 {
0354     struct tx_policy_cache *cache = &priv->tx_policy_cache;
0355     int i;
0356     struct wsm_set_tx_rate_retry_policy arg = {
0357         .num = 0,
0358     };
0359     spin_lock_bh(&cache->lock);
0360 
0361     /* Upload only modified entries. */
0362     for (i = 0; i < TX_POLICY_CACHE_SIZE; ++i) {
0363         struct tx_policy *src = &cache->cache[i].policy;
0364         if (src->retry_count && !src->uploaded) {
0365             struct wsm_tx_rate_retry_policy *dst =
0366                 &arg.tbl[arg.num];
0367             dst->index = i;
0368             dst->short_retries = priv->short_frame_max_tx_count;
0369             dst->long_retries = priv->long_frame_max_tx_count;
0370 
0371             dst->flags = WSM_TX_RATE_POLICY_FLAG_TERMINATE_WHEN_FINISHED |
0372                 WSM_TX_RATE_POLICY_FLAG_COUNT_INITIAL_TRANSMIT;
0373             memcpy(dst->rate_count_indices, src->tbl,
0374                    sizeof(dst->rate_count_indices));
0375             src->uploaded = 1;
0376             ++arg.num;
0377         }
0378     }
0379     spin_unlock_bh(&cache->lock);
0380     cw1200_debug_tx_cache_miss(priv);
0381     pr_debug("[TX policy] Upload %d policies\n", arg.num);
0382     return wsm_set_tx_rate_retry_policy(priv, &arg);
0383 }
0384 
0385 void tx_policy_upload_work(struct work_struct *work)
0386 {
0387     struct cw1200_common *priv =
0388         container_of(work, struct cw1200_common, tx_policy_upload_work);
0389 
0390     pr_debug("[TX] TX policy upload.\n");
0391     tx_policy_upload(priv);
0392 
0393     wsm_unlock_tx(priv);
0394     cw1200_tx_queues_unlock(priv);
0395 }
0396 
0397 /* ******************************************************************** */
0398 /* cw1200 TX implementation                     */
0399 
0400 struct cw1200_txinfo {
0401     struct sk_buff *skb;
0402     unsigned queue;
0403     struct ieee80211_tx_info *tx_info;
0404     const struct ieee80211_rate *rate;
0405     struct ieee80211_hdr *hdr;
0406     size_t hdrlen;
0407     const u8 *da;
0408     struct cw1200_sta_priv *sta_priv;
0409     struct ieee80211_sta *sta;
0410     struct cw1200_txpriv txpriv;
0411 };
0412 
0413 u32 cw1200_rate_mask_to_wsm(struct cw1200_common *priv, u32 rates)
0414 {
0415     u32 ret = 0;
0416     int i;
0417     for (i = 0; i < 32; ++i) {
0418         if (rates & BIT(i))
0419             ret |= BIT(priv->rates[i].hw_value);
0420     }
0421     return ret;
0422 }
0423 
0424 static const struct ieee80211_rate *
0425 cw1200_get_tx_rate(const struct cw1200_common *priv,
0426            const struct ieee80211_tx_rate *rate)
0427 {
0428     if (rate->idx < 0)
0429         return NULL;
0430     if (rate->flags & IEEE80211_TX_RC_MCS)
0431         return &priv->mcs_rates[rate->idx];
0432     return &priv->hw->wiphy->bands[priv->channel->band]->
0433         bitrates[rate->idx];
0434 }
0435 
0436 static int
0437 cw1200_tx_h_calc_link_ids(struct cw1200_common *priv,
0438               struct cw1200_txinfo *t)
0439 {
0440     if (t->sta && t->sta_priv->link_id)
0441         t->txpriv.raw_link_id =
0442                 t->txpriv.link_id =
0443                 t->sta_priv->link_id;
0444     else if (priv->mode != NL80211_IFTYPE_AP)
0445         t->txpriv.raw_link_id =
0446                 t->txpriv.link_id = 0;
0447     else if (is_multicast_ether_addr(t->da)) {
0448         if (priv->enable_beacon) {
0449             t->txpriv.raw_link_id = 0;
0450             t->txpriv.link_id = CW1200_LINK_ID_AFTER_DTIM;
0451         } else {
0452             t->txpriv.raw_link_id = 0;
0453             t->txpriv.link_id = 0;
0454         }
0455     } else {
0456         t->txpriv.link_id = cw1200_find_link_id(priv, t->da);
0457         if (!t->txpriv.link_id)
0458             t->txpriv.link_id = cw1200_alloc_link_id(priv, t->da);
0459         if (!t->txpriv.link_id) {
0460             wiphy_err(priv->hw->wiphy,
0461                   "No more link IDs available.\n");
0462             return -ENOENT;
0463         }
0464         t->txpriv.raw_link_id = t->txpriv.link_id;
0465     }
0466     if (t->txpriv.raw_link_id)
0467         priv->link_id_db[t->txpriv.raw_link_id - 1].timestamp =
0468                 jiffies;
0469     if (t->sta && (t->sta->uapsd_queues & BIT(t->queue)))
0470         t->txpriv.link_id = CW1200_LINK_ID_UAPSD;
0471     return 0;
0472 }
0473 
0474 static void
0475 cw1200_tx_h_pm(struct cw1200_common *priv,
0476            struct cw1200_txinfo *t)
0477 {
0478     if (ieee80211_is_auth(t->hdr->frame_control)) {
0479         u32 mask = ~BIT(t->txpriv.raw_link_id);
0480         spin_lock_bh(&priv->ps_state_lock);
0481         priv->sta_asleep_mask &= mask;
0482         priv->pspoll_mask &= mask;
0483         spin_unlock_bh(&priv->ps_state_lock);
0484     }
0485 }
0486 
0487 static void
0488 cw1200_tx_h_calc_tid(struct cw1200_common *priv,
0489              struct cw1200_txinfo *t)
0490 {
0491     if (ieee80211_is_data_qos(t->hdr->frame_control)) {
0492         u8 *qos = ieee80211_get_qos_ctl(t->hdr);
0493         t->txpriv.tid = qos[0] & IEEE80211_QOS_CTL_TID_MASK;
0494     } else if (ieee80211_is_data(t->hdr->frame_control)) {
0495         t->txpriv.tid = 0;
0496     }
0497 }
0498 
0499 static int
0500 cw1200_tx_h_crypt(struct cw1200_common *priv,
0501           struct cw1200_txinfo *t)
0502 {
0503     if (!t->tx_info->control.hw_key ||
0504         !ieee80211_has_protected(t->hdr->frame_control))
0505         return 0;
0506 
0507     t->hdrlen += t->tx_info->control.hw_key->iv_len;
0508     skb_put(t->skb, t->tx_info->control.hw_key->icv_len);
0509 
0510     if (t->tx_info->control.hw_key->cipher == WLAN_CIPHER_SUITE_TKIP)
0511         skb_put(t->skb, 8); /* MIC space */
0512 
0513     return 0;
0514 }
0515 
0516 static int
0517 cw1200_tx_h_align(struct cw1200_common *priv,
0518           struct cw1200_txinfo *t,
0519           u8 *flags)
0520 {
0521     size_t offset = (size_t)t->skb->data & 3;
0522 
0523     if (!offset)
0524         return 0;
0525 
0526     if (offset & 1) {
0527         wiphy_err(priv->hw->wiphy,
0528               "Bug: attempt to transmit a frame with wrong alignment: %zu\n",
0529               offset);
0530         return -EINVAL;
0531     }
0532 
0533     if (skb_headroom(t->skb) < offset) {
0534         wiphy_err(priv->hw->wiphy,
0535               "Bug: no space allocated for DMA alignment. headroom: %d\n",
0536               skb_headroom(t->skb));
0537         return -ENOMEM;
0538     }
0539     skb_push(t->skb, offset);
0540     t->hdrlen += offset;
0541     t->txpriv.offset += offset;
0542     *flags |= WSM_TX_2BYTES_SHIFT;
0543     cw1200_debug_tx_align(priv);
0544     return 0;
0545 }
0546 
0547 static int
0548 cw1200_tx_h_action(struct cw1200_common *priv,
0549            struct cw1200_txinfo *t)
0550 {
0551     struct ieee80211_mgmt *mgmt =
0552         (struct ieee80211_mgmt *)t->hdr;
0553     if (ieee80211_is_action(t->hdr->frame_control) &&
0554         mgmt->u.action.category == WLAN_CATEGORY_BACK)
0555         return 1;
0556     else
0557         return 0;
0558 }
0559 
0560 /* Add WSM header */
0561 static struct wsm_tx *
0562 cw1200_tx_h_wsm(struct cw1200_common *priv,
0563         struct cw1200_txinfo *t)
0564 {
0565     struct wsm_tx *wsm;
0566 
0567     if (skb_headroom(t->skb) < sizeof(struct wsm_tx)) {
0568         wiphy_err(priv->hw->wiphy,
0569               "Bug: no space allocated for WSM header. headroom: %d\n",
0570               skb_headroom(t->skb));
0571         return NULL;
0572     }
0573 
0574     wsm = skb_push(t->skb, sizeof(struct wsm_tx));
0575     t->txpriv.offset += sizeof(struct wsm_tx);
0576     memset(wsm, 0, sizeof(*wsm));
0577     wsm->hdr.len = __cpu_to_le16(t->skb->len);
0578     wsm->hdr.id = __cpu_to_le16(0x0004);
0579     wsm->queue_id = wsm_queue_id_to_wsm(t->queue);
0580     return wsm;
0581 }
0582 
0583 /* BT Coex specific handling */
0584 static void
0585 cw1200_tx_h_bt(struct cw1200_common *priv,
0586            struct cw1200_txinfo *t,
0587            struct wsm_tx *wsm)
0588 {
0589     u8 priority = 0;
0590 
0591     if (!priv->bt_present)
0592         return;
0593 
0594     if (ieee80211_is_nullfunc(t->hdr->frame_control)) {
0595         priority = WSM_EPTA_PRIORITY_MGT;
0596     } else if (ieee80211_is_data(t->hdr->frame_control)) {
0597         /* Skip LLC SNAP header (+6) */
0598         u8 *payload = &t->skb->data[t->hdrlen];
0599         __be16 *ethertype = (__be16 *)&payload[6];
0600         if (be16_to_cpu(*ethertype) == ETH_P_PAE)
0601             priority = WSM_EPTA_PRIORITY_EAPOL;
0602     } else if (ieee80211_is_assoc_req(t->hdr->frame_control) ||
0603         ieee80211_is_reassoc_req(t->hdr->frame_control)) {
0604         struct ieee80211_mgmt *mgt_frame =
0605                 (struct ieee80211_mgmt *)t->hdr;
0606 
0607         if (le16_to_cpu(mgt_frame->u.assoc_req.listen_interval) <
0608                         priv->listen_interval) {
0609             pr_debug("Modified Listen Interval to %d from %d\n",
0610                  priv->listen_interval,
0611                  mgt_frame->u.assoc_req.listen_interval);
0612             /* Replace listen interval derieved from
0613              * the one read from SDD
0614              */
0615             mgt_frame->u.assoc_req.listen_interval = cpu_to_le16(priv->listen_interval);
0616         }
0617     }
0618 
0619     if (!priority) {
0620         if (ieee80211_is_action(t->hdr->frame_control))
0621             priority = WSM_EPTA_PRIORITY_ACTION;
0622         else if (ieee80211_is_mgmt(t->hdr->frame_control))
0623             priority = WSM_EPTA_PRIORITY_MGT;
0624         else if (wsm->queue_id == WSM_QUEUE_VOICE)
0625             priority = WSM_EPTA_PRIORITY_VOICE;
0626         else if (wsm->queue_id == WSM_QUEUE_VIDEO)
0627             priority = WSM_EPTA_PRIORITY_VIDEO;
0628         else
0629             priority = WSM_EPTA_PRIORITY_DATA;
0630     }
0631 
0632     pr_debug("[TX] EPTA priority %d.\n", priority);
0633 
0634     wsm->flags |= priority << 1;
0635 }
0636 
0637 static int
0638 cw1200_tx_h_rate_policy(struct cw1200_common *priv,
0639             struct cw1200_txinfo *t,
0640             struct wsm_tx *wsm)
0641 {
0642     bool tx_policy_renew = false;
0643 
0644     t->txpriv.rate_id = tx_policy_get(priv,
0645         t->tx_info->control.rates, IEEE80211_TX_MAX_RATES,
0646         &tx_policy_renew);
0647     if (t->txpriv.rate_id == CW1200_INVALID_RATE_ID)
0648         return -EFAULT;
0649 
0650     wsm->flags |= t->txpriv.rate_id << 4;
0651 
0652     t->rate = cw1200_get_tx_rate(priv,
0653         &t->tx_info->control.rates[0]);
0654     wsm->max_tx_rate = t->rate->hw_value;
0655     if (t->rate->flags & IEEE80211_TX_RC_MCS) {
0656         if (cw1200_ht_greenfield(&priv->ht_info))
0657             wsm->ht_tx_parameters |=
0658                 __cpu_to_le32(WSM_HT_TX_GREENFIELD);
0659         else
0660             wsm->ht_tx_parameters |=
0661                 __cpu_to_le32(WSM_HT_TX_MIXED);
0662     }
0663 
0664     if (tx_policy_renew) {
0665         pr_debug("[TX] TX policy renew.\n");
0666         /* It's not so optimal to stop TX queues every now and then.
0667          * Better to reimplement task scheduling with
0668          * a counter. TODO.
0669          */
0670         wsm_lock_tx_async(priv);
0671         cw1200_tx_queues_lock(priv);
0672         if (queue_work(priv->workqueue,
0673                    &priv->tx_policy_upload_work) <= 0) {
0674             cw1200_tx_queues_unlock(priv);
0675             wsm_unlock_tx(priv);
0676         }
0677     }
0678     return 0;
0679 }
0680 
0681 static bool
0682 cw1200_tx_h_pm_state(struct cw1200_common *priv,
0683              struct cw1200_txinfo *t)
0684 {
0685     int was_buffered = 1;
0686 
0687     if (t->txpriv.link_id == CW1200_LINK_ID_AFTER_DTIM &&
0688         !priv->buffered_multicasts) {
0689         priv->buffered_multicasts = true;
0690         if (priv->sta_asleep_mask)
0691             queue_work(priv->workqueue,
0692                    &priv->multicast_start_work);
0693     }
0694 
0695     if (t->txpriv.raw_link_id && t->txpriv.tid < CW1200_MAX_TID)
0696         was_buffered = priv->link_id_db[t->txpriv.raw_link_id - 1].buffered[t->txpriv.tid]++;
0697 
0698     return !was_buffered;
0699 }
0700 
0701 /* ******************************************************************** */
0702 
0703 void cw1200_tx(struct ieee80211_hw *dev,
0704            struct ieee80211_tx_control *control,
0705            struct sk_buff *skb)
0706 {
0707     struct cw1200_common *priv = dev->priv;
0708     struct cw1200_txinfo t = {
0709         .skb = skb,
0710         .queue = skb_get_queue_mapping(skb),
0711         .tx_info = IEEE80211_SKB_CB(skb),
0712         .hdr = (struct ieee80211_hdr *)skb->data,
0713         .txpriv.tid = CW1200_MAX_TID,
0714         .txpriv.rate_id = CW1200_INVALID_RATE_ID,
0715     };
0716     struct ieee80211_sta *sta;
0717     struct wsm_tx *wsm;
0718     bool tid_update = false;
0719     u8 flags = 0;
0720     int ret;
0721 
0722     if (priv->bh_error)
0723         goto drop;
0724 
0725     t.hdrlen = ieee80211_hdrlen(t.hdr->frame_control);
0726     t.da = ieee80211_get_DA(t.hdr);
0727     if (control) {
0728         t.sta = control->sta;
0729         t.sta_priv = (struct cw1200_sta_priv *)&t.sta->drv_priv;
0730     }
0731 
0732     if (WARN_ON(t.queue >= 4))
0733         goto drop;
0734 
0735     ret = cw1200_tx_h_calc_link_ids(priv, &t);
0736     if (ret)
0737         goto drop;
0738 
0739     pr_debug("[TX] TX %d bytes (queue: %d, link_id: %d (%d)).\n",
0740          skb->len, t.queue, t.txpriv.link_id,
0741          t.txpriv.raw_link_id);
0742 
0743     cw1200_tx_h_pm(priv, &t);
0744     cw1200_tx_h_calc_tid(priv, &t);
0745     ret = cw1200_tx_h_crypt(priv, &t);
0746     if (ret)
0747         goto drop;
0748     ret = cw1200_tx_h_align(priv, &t, &flags);
0749     if (ret)
0750         goto drop;
0751     ret = cw1200_tx_h_action(priv, &t);
0752     if (ret)
0753         goto drop;
0754     wsm = cw1200_tx_h_wsm(priv, &t);
0755     if (!wsm) {
0756         ret = -ENOMEM;
0757         goto drop;
0758     }
0759     wsm->flags |= flags;
0760     cw1200_tx_h_bt(priv, &t, wsm);
0761     ret = cw1200_tx_h_rate_policy(priv, &t, wsm);
0762     if (ret)
0763         goto drop;
0764 
0765     rcu_read_lock();
0766     sta = rcu_dereference(t.sta);
0767 
0768     spin_lock_bh(&priv->ps_state_lock);
0769     {
0770         tid_update = cw1200_tx_h_pm_state(priv, &t);
0771         BUG_ON(cw1200_queue_put(&priv->tx_queue[t.queue],
0772                     t.skb, &t.txpriv));
0773     }
0774     spin_unlock_bh(&priv->ps_state_lock);
0775 
0776     if (tid_update && sta)
0777         ieee80211_sta_set_buffered(sta, t.txpriv.tid, true);
0778 
0779     rcu_read_unlock();
0780 
0781     cw1200_bh_wakeup(priv);
0782 
0783     return;
0784 
0785 drop:
0786     cw1200_skb_dtor(priv, skb, &t.txpriv);
0787     return;
0788 }
0789 
0790 /* ******************************************************************** */
0791 
0792 static int cw1200_handle_action_rx(struct cw1200_common *priv,
0793                    struct sk_buff *skb)
0794 {
0795     struct ieee80211_mgmt *mgmt = (void *)skb->data;
0796 
0797     /* Filter block ACK negotiation: fully controlled by firmware */
0798     if (mgmt->u.action.category == WLAN_CATEGORY_BACK)
0799         return 1;
0800 
0801     return 0;
0802 }
0803 
0804 static int cw1200_handle_pspoll(struct cw1200_common *priv,
0805                 struct sk_buff *skb)
0806 {
0807     struct ieee80211_sta *sta;
0808     struct ieee80211_pspoll *pspoll = (struct ieee80211_pspoll *)skb->data;
0809     int link_id = 0;
0810     u32 pspoll_mask = 0;
0811     int drop = 1;
0812     int i;
0813 
0814     if (priv->join_status != CW1200_JOIN_STATUS_AP)
0815         goto done;
0816     if (memcmp(priv->vif->addr, pspoll->bssid, ETH_ALEN))
0817         goto done;
0818 
0819     rcu_read_lock();
0820     sta = ieee80211_find_sta(priv->vif, pspoll->ta);
0821     if (sta) {
0822         struct cw1200_sta_priv *sta_priv;
0823         sta_priv = (struct cw1200_sta_priv *)&sta->drv_priv;
0824         link_id = sta_priv->link_id;
0825         pspoll_mask = BIT(sta_priv->link_id);
0826     }
0827     rcu_read_unlock();
0828     if (!link_id)
0829         goto done;
0830 
0831     priv->pspoll_mask |= pspoll_mask;
0832     drop = 0;
0833 
0834     /* Do not report pspols if data for given link id is queued already. */
0835     for (i = 0; i < 4; ++i) {
0836         if (cw1200_queue_get_num_queued(&priv->tx_queue[i],
0837                         pspoll_mask)) {
0838             cw1200_bh_wakeup(priv);
0839             drop = 1;
0840             break;
0841         }
0842     }
0843     pr_debug("[RX] PSPOLL: %s\n", drop ? "local" : "fwd");
0844 done:
0845     return drop;
0846 }
0847 
0848 /* ******************************************************************** */
0849 
0850 void cw1200_tx_confirm_cb(struct cw1200_common *priv,
0851               int link_id,
0852               struct wsm_tx_confirm *arg)
0853 {
0854     u8 queue_id = cw1200_queue_get_queue_id(arg->packet_id);
0855     struct cw1200_queue *queue = &priv->tx_queue[queue_id];
0856     struct sk_buff *skb;
0857     const struct cw1200_txpriv *txpriv;
0858 
0859     pr_debug("[TX] TX confirm: %d, %d.\n",
0860          arg->status, arg->ack_failures);
0861 
0862     if (priv->mode == NL80211_IFTYPE_UNSPECIFIED) {
0863         /* STA is stopped. */
0864         return;
0865     }
0866 
0867     if (WARN_ON(queue_id >= 4))
0868         return;
0869 
0870     if (arg->status)
0871         pr_debug("TX failed: %d.\n", arg->status);
0872 
0873     if ((arg->status == WSM_REQUEUE) &&
0874         (arg->flags & WSM_TX_STATUS_REQUEUE)) {
0875         /* "Requeue" means "implicit suspend" */
0876         struct wsm_suspend_resume suspend = {
0877             .link_id = link_id,
0878             .stop = 1,
0879             .multicast = !link_id,
0880         };
0881         cw1200_suspend_resume(priv, &suspend);
0882         wiphy_warn(priv->hw->wiphy, "Requeue for link_id %d (try %d). STAs asleep: 0x%.8X\n",
0883                link_id,
0884                cw1200_queue_get_generation(arg->packet_id) + 1,
0885                priv->sta_asleep_mask);
0886         cw1200_queue_requeue(queue, arg->packet_id);
0887         spin_lock_bh(&priv->ps_state_lock);
0888         if (!link_id) {
0889             priv->buffered_multicasts = true;
0890             if (priv->sta_asleep_mask) {
0891                 queue_work(priv->workqueue,
0892                        &priv->multicast_start_work);
0893             }
0894         }
0895         spin_unlock_bh(&priv->ps_state_lock);
0896     } else if (!cw1200_queue_get_skb(queue, arg->packet_id,
0897                      &skb, &txpriv)) {
0898         struct ieee80211_tx_info *tx = IEEE80211_SKB_CB(skb);
0899         int tx_count = arg->ack_failures;
0900         u8 ht_flags = 0;
0901         int i;
0902 
0903         if (cw1200_ht_greenfield(&priv->ht_info))
0904             ht_flags |= IEEE80211_TX_RC_GREEN_FIELD;
0905 
0906         spin_lock(&priv->bss_loss_lock);
0907         if (priv->bss_loss_state &&
0908             arg->packet_id == priv->bss_loss_confirm_id) {
0909             if (arg->status) {
0910                 /* Recovery failed */
0911                 __cw1200_cqm_bssloss_sm(priv, 0, 0, 1);
0912             } else {
0913                 /* Recovery succeeded */
0914                 __cw1200_cqm_bssloss_sm(priv, 0, 1, 0);
0915             }
0916         }
0917         spin_unlock(&priv->bss_loss_lock);
0918 
0919         if (!arg->status) {
0920             tx->flags |= IEEE80211_TX_STAT_ACK;
0921             ++tx_count;
0922             cw1200_debug_txed(priv);
0923             if (arg->flags & WSM_TX_STATUS_AGGREGATION) {
0924                 /* Do not report aggregation to mac80211:
0925                  * it confuses minstrel a lot.
0926                  */
0927                 /* tx->flags |= IEEE80211_TX_STAT_AMPDU; */
0928                 cw1200_debug_txed_agg(priv);
0929             }
0930         } else {
0931             if (tx_count)
0932                 ++tx_count;
0933         }
0934 
0935         for (i = 0; i < IEEE80211_TX_MAX_RATES; ++i) {
0936             if (tx->status.rates[i].count >= tx_count) {
0937                 tx->status.rates[i].count = tx_count;
0938                 break;
0939             }
0940             tx_count -= tx->status.rates[i].count;
0941             if (tx->status.rates[i].flags & IEEE80211_TX_RC_MCS)
0942                 tx->status.rates[i].flags |= ht_flags;
0943         }
0944 
0945         for (++i; i < IEEE80211_TX_MAX_RATES; ++i) {
0946             tx->status.rates[i].count = 0;
0947             tx->status.rates[i].idx = -1;
0948         }
0949 
0950         /* Pull off any crypto trailers that we added on */
0951         if (tx->control.hw_key) {
0952             skb_trim(skb, skb->len - tx->control.hw_key->icv_len);
0953             if (tx->control.hw_key->cipher == WLAN_CIPHER_SUITE_TKIP)
0954                 skb_trim(skb, skb->len - 8); /* MIC space */
0955         }
0956         cw1200_queue_remove(queue, arg->packet_id);
0957     }
0958     /* XXX TODO:  Only wake if there are pending transmits.. */
0959     cw1200_bh_wakeup(priv);
0960 }
0961 
0962 static void cw1200_notify_buffered_tx(struct cw1200_common *priv,
0963                    struct sk_buff *skb, int link_id, int tid)
0964 {
0965     struct ieee80211_sta *sta;
0966     struct ieee80211_hdr *hdr;
0967     u8 *buffered;
0968     u8 still_buffered = 0;
0969 
0970     if (link_id && tid < CW1200_MAX_TID) {
0971         buffered = priv->link_id_db
0972                 [link_id - 1].buffered;
0973 
0974         spin_lock_bh(&priv->ps_state_lock);
0975         if (!WARN_ON(!buffered[tid]))
0976             still_buffered = --buffered[tid];
0977         spin_unlock_bh(&priv->ps_state_lock);
0978 
0979         if (!still_buffered && tid < CW1200_MAX_TID) {
0980             hdr = (struct ieee80211_hdr *)skb->data;
0981             rcu_read_lock();
0982             sta = ieee80211_find_sta(priv->vif, hdr->addr1);
0983             if (sta)
0984                 ieee80211_sta_set_buffered(sta, tid, false);
0985             rcu_read_unlock();
0986         }
0987     }
0988 }
0989 
0990 void cw1200_skb_dtor(struct cw1200_common *priv,
0991              struct sk_buff *skb,
0992              const struct cw1200_txpriv *txpriv)
0993 {
0994     skb_pull(skb, txpriv->offset);
0995     if (txpriv->rate_id != CW1200_INVALID_RATE_ID) {
0996         cw1200_notify_buffered_tx(priv, skb,
0997                       txpriv->raw_link_id, txpriv->tid);
0998         tx_policy_put(priv, txpriv->rate_id);
0999     }
1000     ieee80211_tx_status(priv->hw, skb);
1001 }
1002 
1003 void cw1200_rx_cb(struct cw1200_common *priv,
1004           struct wsm_rx *arg,
1005           int link_id,
1006           struct sk_buff **skb_p)
1007 {
1008     struct sk_buff *skb = *skb_p;
1009     struct ieee80211_rx_status *hdr = IEEE80211_SKB_RXCB(skb);
1010     struct ieee80211_hdr *frame = (struct ieee80211_hdr *)skb->data;
1011     struct ieee80211_mgmt *mgmt = (struct ieee80211_mgmt *)skb->data;
1012     struct cw1200_link_entry *entry = NULL;
1013     unsigned long grace_period;
1014 
1015     bool early_data = false;
1016     bool p2p = priv->vif && priv->vif->p2p;
1017     size_t hdrlen;
1018     hdr->flag = 0;
1019 
1020     if (priv->mode == NL80211_IFTYPE_UNSPECIFIED) {
1021         /* STA is stopped. */
1022         goto drop;
1023     }
1024 
1025     if (link_id && link_id <= CW1200_MAX_STA_IN_AP_MODE) {
1026         entry = &priv->link_id_db[link_id - 1];
1027         if (entry->status == CW1200_LINK_SOFT &&
1028             ieee80211_is_data(frame->frame_control))
1029             early_data = true;
1030         entry->timestamp = jiffies;
1031     } else if (p2p &&
1032            ieee80211_is_action(frame->frame_control) &&
1033            (mgmt->u.action.category == WLAN_CATEGORY_PUBLIC)) {
1034         pr_debug("[RX] Going to MAP&RESET link ID\n");
1035         WARN_ON(work_pending(&priv->linkid_reset_work));
1036         memcpy(&priv->action_frame_sa[0],
1037                ieee80211_get_SA(frame), ETH_ALEN);
1038         priv->action_linkid = 0;
1039         schedule_work(&priv->linkid_reset_work);
1040     }
1041 
1042     if (link_id && p2p &&
1043         ieee80211_is_action(frame->frame_control) &&
1044         (mgmt->u.action.category == WLAN_CATEGORY_PUBLIC)) {
1045         /* Link ID already exists for the ACTION frame.
1046          * Reset and Remap
1047          */
1048         WARN_ON(work_pending(&priv->linkid_reset_work));
1049         memcpy(&priv->action_frame_sa[0],
1050                ieee80211_get_SA(frame), ETH_ALEN);
1051         priv->action_linkid = link_id;
1052         schedule_work(&priv->linkid_reset_work);
1053     }
1054     if (arg->status) {
1055         if (arg->status == WSM_STATUS_MICFAILURE) {
1056             pr_debug("[RX] MIC failure.\n");
1057             hdr->flag |= RX_FLAG_MMIC_ERROR;
1058         } else if (arg->status == WSM_STATUS_NO_KEY_FOUND) {
1059             pr_debug("[RX] No key found.\n");
1060             goto drop;
1061         } else {
1062             pr_debug("[RX] Receive failure: %d.\n",
1063                  arg->status);
1064             goto drop;
1065         }
1066     }
1067 
1068     if (skb->len < sizeof(struct ieee80211_pspoll)) {
1069         wiphy_warn(priv->hw->wiphy, "Malformed SDU rx'ed. Size is lesser than IEEE header.\n");
1070         goto drop;
1071     }
1072 
1073     if (ieee80211_is_pspoll(frame->frame_control))
1074         if (cw1200_handle_pspoll(priv, skb))
1075             goto drop;
1076 
1077     hdr->band = ((arg->channel_number & 0xff00) ||
1078              (arg->channel_number > 14)) ?
1079             NL80211_BAND_5GHZ : NL80211_BAND_2GHZ;
1080     hdr->freq = ieee80211_channel_to_frequency(
1081             arg->channel_number,
1082             hdr->band);
1083 
1084     if (arg->rx_rate >= 14) {
1085         hdr->encoding = RX_ENC_HT;
1086         hdr->rate_idx = arg->rx_rate - 14;
1087     } else if (arg->rx_rate >= 4) {
1088         hdr->rate_idx = arg->rx_rate - 2;
1089     } else {
1090         hdr->rate_idx = arg->rx_rate;
1091     }
1092 
1093     hdr->signal = (s8)arg->rcpi_rssi;
1094     hdr->antenna = 0;
1095 
1096     hdrlen = ieee80211_hdrlen(frame->frame_control);
1097 
1098     if (WSM_RX_STATUS_ENCRYPTION(arg->flags)) {
1099         size_t iv_len = 0, icv_len = 0;
1100 
1101         hdr->flag |= RX_FLAG_DECRYPTED | RX_FLAG_IV_STRIPPED;
1102 
1103         /* Oops... There is no fast way to ask mac80211 about
1104          * IV/ICV lengths. Even defineas are not exposed.
1105          */
1106         switch (WSM_RX_STATUS_ENCRYPTION(arg->flags)) {
1107         case WSM_RX_STATUS_WEP:
1108             iv_len = 4 /* WEP_IV_LEN */;
1109             icv_len = 4 /* WEP_ICV_LEN */;
1110             break;
1111         case WSM_RX_STATUS_TKIP:
1112             iv_len = 8 /* TKIP_IV_LEN */;
1113             icv_len = 4 /* TKIP_ICV_LEN */
1114                 + 8 /*MICHAEL_MIC_LEN*/;
1115             hdr->flag |= RX_FLAG_MMIC_STRIPPED;
1116             break;
1117         case WSM_RX_STATUS_AES:
1118             iv_len = 8 /* CCMP_HDR_LEN */;
1119             icv_len = 8 /* CCMP_MIC_LEN */;
1120             break;
1121         case WSM_RX_STATUS_WAPI:
1122             iv_len = 18 /* WAPI_HDR_LEN */;
1123             icv_len = 16 /* WAPI_MIC_LEN */;
1124             break;
1125         default:
1126             pr_warn("Unknown encryption type %d\n",
1127                 WSM_RX_STATUS_ENCRYPTION(arg->flags));
1128             goto drop;
1129         }
1130 
1131         /* Firmware strips ICV in case of MIC failure. */
1132         if (arg->status == WSM_STATUS_MICFAILURE)
1133             icv_len = 0;
1134 
1135         if (skb->len < hdrlen + iv_len + icv_len) {
1136             wiphy_warn(priv->hw->wiphy, "Malformed SDU rx'ed. Size is lesser than crypto headers.\n");
1137             goto drop;
1138         }
1139 
1140         /* Remove IV, ICV and MIC */
1141         skb_trim(skb, skb->len - icv_len);
1142         memmove(skb->data + iv_len, skb->data, hdrlen);
1143         skb_pull(skb, iv_len);
1144     }
1145 
1146     /* Remove TSF from the end of frame */
1147     if (arg->flags & WSM_RX_STATUS_TSF_INCLUDED) {
1148         memcpy(&hdr->mactime, skb->data + skb->len - 8, 8);
1149         hdr->mactime = le64_to_cpu(hdr->mactime);
1150         if (skb->len >= 8)
1151             skb_trim(skb, skb->len - 8);
1152     } else {
1153         hdr->mactime = 0;
1154     }
1155 
1156     cw1200_debug_rxed(priv);
1157     if (arg->flags & WSM_RX_STATUS_AGGREGATE)
1158         cw1200_debug_rxed_agg(priv);
1159 
1160     if (ieee80211_is_action(frame->frame_control) &&
1161         (arg->flags & WSM_RX_STATUS_ADDRESS1)) {
1162         if (cw1200_handle_action_rx(priv, skb))
1163             return;
1164     } else if (ieee80211_is_beacon(frame->frame_control) &&
1165            !arg->status && priv->vif &&
1166            ether_addr_equal(ieee80211_get_SA(frame), priv->vif->bss_conf.bssid)) {
1167         const u8 *tim_ie;
1168         u8 *ies = ((struct ieee80211_mgmt *)
1169               (skb->data))->u.beacon.variable;
1170         size_t ies_len = skb->len - (ies - (u8 *)(skb->data));
1171 
1172         tim_ie = cfg80211_find_ie(WLAN_EID_TIM, ies, ies_len);
1173         if (tim_ie) {
1174             struct ieee80211_tim_ie *tim =
1175                 (struct ieee80211_tim_ie *)&tim_ie[2];
1176 
1177             if (priv->join_dtim_period != tim->dtim_period) {
1178                 priv->join_dtim_period = tim->dtim_period;
1179                 queue_work(priv->workqueue,
1180                        &priv->set_beacon_wakeup_period_work);
1181             }
1182         }
1183 
1184         /* Disable beacon filter once we're associated... */
1185         if (priv->disable_beacon_filter &&
1186             (priv->vif->cfg.assoc ||
1187              priv->vif->cfg.ibss_joined)) {
1188             priv->disable_beacon_filter = false;
1189             queue_work(priv->workqueue,
1190                    &priv->update_filtering_work);
1191         }
1192     }
1193 
1194     /* Stay awake after frame is received to give
1195      * userspace chance to react and acquire appropriate
1196      * wakelock.
1197      */
1198     if (ieee80211_is_auth(frame->frame_control))
1199         grace_period = 5 * HZ;
1200     else if (ieee80211_is_deauth(frame->frame_control))
1201         grace_period = 5 * HZ;
1202     else
1203         grace_period = 1 * HZ;
1204     cw1200_pm_stay_awake(&priv->pm_state, grace_period);
1205 
1206     if (early_data) {
1207         spin_lock_bh(&priv->ps_state_lock);
1208         /* Double-check status with lock held */
1209         if (entry->status == CW1200_LINK_SOFT)
1210             skb_queue_tail(&entry->rx_queue, skb);
1211         else
1212             ieee80211_rx_irqsafe(priv->hw, skb);
1213         spin_unlock_bh(&priv->ps_state_lock);
1214     } else {
1215         ieee80211_rx_irqsafe(priv->hw, skb);
1216     }
1217     *skb_p = NULL;
1218 
1219     return;
1220 
1221 drop:
1222     /* TODO: update failure counters */
1223     return;
1224 }
1225 
1226 /* ******************************************************************** */
1227 /* Security                             */
1228 
1229 int cw1200_alloc_key(struct cw1200_common *priv)
1230 {
1231     int idx;
1232 
1233     idx = ffs(~priv->key_map) - 1;
1234     if (idx < 0 || idx > WSM_KEY_MAX_INDEX)
1235         return -1;
1236 
1237     priv->key_map |= BIT(idx);
1238     priv->keys[idx].index = idx;
1239     return idx;
1240 }
1241 
1242 void cw1200_free_key(struct cw1200_common *priv, int idx)
1243 {
1244     BUG_ON(!(priv->key_map & BIT(idx)));
1245     memset(&priv->keys[idx], 0, sizeof(priv->keys[idx]));
1246     priv->key_map &= ~BIT(idx);
1247 }
1248 
1249 void cw1200_free_keys(struct cw1200_common *priv)
1250 {
1251     memset(&priv->keys, 0, sizeof(priv->keys));
1252     priv->key_map = 0;
1253 }
1254 
1255 int cw1200_upload_keys(struct cw1200_common *priv)
1256 {
1257     int idx, ret = 0;
1258     for (idx = 0; idx <= WSM_KEY_MAX_INDEX; ++idx)
1259         if (priv->key_map & BIT(idx)) {
1260             ret = wsm_add_key(priv, &priv->keys[idx]);
1261             if (ret < 0)
1262                 break;
1263         }
1264     return ret;
1265 }
1266 
1267 /* Workaround for WFD test case 6.1.10 */
1268 void cw1200_link_id_reset(struct work_struct *work)
1269 {
1270     struct cw1200_common *priv =
1271         container_of(work, struct cw1200_common, linkid_reset_work);
1272     int temp_linkid;
1273 
1274     if (!priv->action_linkid) {
1275         /* In GO mode we can receive ACTION frames without a linkID */
1276         temp_linkid = cw1200_alloc_link_id(priv,
1277                 &priv->action_frame_sa[0]);
1278         WARN_ON(!temp_linkid);
1279         if (temp_linkid) {
1280             /* Make sure we execute the WQ */
1281             flush_workqueue(priv->workqueue);
1282             /* Release the link ID */
1283             spin_lock_bh(&priv->ps_state_lock);
1284             priv->link_id_db[temp_linkid - 1].prev_status =
1285                 priv->link_id_db[temp_linkid - 1].status;
1286             priv->link_id_db[temp_linkid - 1].status =
1287                 CW1200_LINK_RESET;
1288             spin_unlock_bh(&priv->ps_state_lock);
1289             wsm_lock_tx_async(priv);
1290             if (queue_work(priv->workqueue,
1291                        &priv->link_id_work) <= 0)
1292                 wsm_unlock_tx(priv);
1293         }
1294     } else {
1295         spin_lock_bh(&priv->ps_state_lock);
1296         priv->link_id_db[priv->action_linkid - 1].prev_status =
1297             priv->link_id_db[priv->action_linkid - 1].status;
1298         priv->link_id_db[priv->action_linkid - 1].status =
1299             CW1200_LINK_RESET_REMAP;
1300         spin_unlock_bh(&priv->ps_state_lock);
1301         wsm_lock_tx_async(priv);
1302         if (queue_work(priv->workqueue, &priv->link_id_work) <= 0)
1303             wsm_unlock_tx(priv);
1304         flush_workqueue(priv->workqueue);
1305     }
1306 }
1307 
1308 int cw1200_find_link_id(struct cw1200_common *priv, const u8 *mac)
1309 {
1310     int i, ret = 0;
1311     spin_lock_bh(&priv->ps_state_lock);
1312     for (i = 0; i < CW1200_MAX_STA_IN_AP_MODE; ++i) {
1313         if (!memcmp(mac, priv->link_id_db[i].mac, ETH_ALEN) &&
1314             priv->link_id_db[i].status) {
1315             priv->link_id_db[i].timestamp = jiffies;
1316             ret = i + 1;
1317             break;
1318         }
1319     }
1320     spin_unlock_bh(&priv->ps_state_lock);
1321     return ret;
1322 }
1323 
1324 int cw1200_alloc_link_id(struct cw1200_common *priv, const u8 *mac)
1325 {
1326     int i, ret = 0;
1327     unsigned long max_inactivity = 0;
1328     unsigned long now = jiffies;
1329 
1330     spin_lock_bh(&priv->ps_state_lock);
1331     for (i = 0; i < CW1200_MAX_STA_IN_AP_MODE; ++i) {
1332         if (!priv->link_id_db[i].status) {
1333             ret = i + 1;
1334             break;
1335         } else if (priv->link_id_db[i].status != CW1200_LINK_HARD &&
1336                !priv->tx_queue_stats.link_map_cache[i + 1]) {
1337             unsigned long inactivity =
1338                 now - priv->link_id_db[i].timestamp;
1339             if (inactivity < max_inactivity)
1340                 continue;
1341             max_inactivity = inactivity;
1342             ret = i + 1;
1343         }
1344     }
1345     if (ret) {
1346         struct cw1200_link_entry *entry = &priv->link_id_db[ret - 1];
1347         pr_debug("[AP] STA added, link_id: %d\n", ret);
1348         entry->status = CW1200_LINK_RESERVE;
1349         memcpy(&entry->mac, mac, ETH_ALEN);
1350         memset(&entry->buffered, 0, CW1200_MAX_TID);
1351         skb_queue_head_init(&entry->rx_queue);
1352         wsm_lock_tx_async(priv);
1353         if (queue_work(priv->workqueue, &priv->link_id_work) <= 0)
1354             wsm_unlock_tx(priv);
1355     } else {
1356         wiphy_info(priv->hw->wiphy,
1357                "[AP] Early: no more link IDs available.\n");
1358     }
1359 
1360     spin_unlock_bh(&priv->ps_state_lock);
1361     return ret;
1362 }
1363 
1364 void cw1200_link_id_work(struct work_struct *work)
1365 {
1366     struct cw1200_common *priv =
1367         container_of(work, struct cw1200_common, link_id_work);
1368     wsm_flush_tx(priv);
1369     cw1200_link_id_gc_work(&priv->link_id_gc_work.work);
1370     wsm_unlock_tx(priv);
1371 }
1372 
1373 void cw1200_link_id_gc_work(struct work_struct *work)
1374 {
1375     struct cw1200_common *priv =
1376         container_of(work, struct cw1200_common, link_id_gc_work.work);
1377     struct wsm_reset reset = {
1378         .reset_statistics = false,
1379     };
1380     struct wsm_map_link map_link = {
1381         .link_id = 0,
1382     };
1383     unsigned long now = jiffies;
1384     unsigned long next_gc = -1;
1385     long ttl;
1386     bool need_reset;
1387     u32 mask;
1388     int i;
1389 
1390     if (priv->join_status != CW1200_JOIN_STATUS_AP)
1391         return;
1392 
1393     wsm_lock_tx(priv);
1394     spin_lock_bh(&priv->ps_state_lock);
1395     for (i = 0; i < CW1200_MAX_STA_IN_AP_MODE; ++i) {
1396         need_reset = false;
1397         mask = BIT(i + 1);
1398         if (priv->link_id_db[i].status == CW1200_LINK_RESERVE ||
1399             (priv->link_id_db[i].status == CW1200_LINK_HARD &&
1400              !(priv->link_id_map & mask))) {
1401             if (priv->link_id_map & mask) {
1402                 priv->sta_asleep_mask &= ~mask;
1403                 priv->pspoll_mask &= ~mask;
1404                 need_reset = true;
1405             }
1406             priv->link_id_map |= mask;
1407             if (priv->link_id_db[i].status != CW1200_LINK_HARD)
1408                 priv->link_id_db[i].status = CW1200_LINK_SOFT;
1409             memcpy(map_link.mac_addr, priv->link_id_db[i].mac,
1410                    ETH_ALEN);
1411             spin_unlock_bh(&priv->ps_state_lock);
1412             if (need_reset) {
1413                 reset.link_id = i + 1;
1414                 wsm_reset(priv, &reset);
1415             }
1416             map_link.link_id = i + 1;
1417             wsm_map_link(priv, &map_link);
1418             next_gc = min(next_gc, CW1200_LINK_ID_GC_TIMEOUT);
1419             spin_lock_bh(&priv->ps_state_lock);
1420         } else if (priv->link_id_db[i].status == CW1200_LINK_SOFT) {
1421             ttl = priv->link_id_db[i].timestamp - now +
1422                     CW1200_LINK_ID_GC_TIMEOUT;
1423             if (ttl <= 0) {
1424                 need_reset = true;
1425                 priv->link_id_db[i].status = CW1200_LINK_OFF;
1426                 priv->link_id_map &= ~mask;
1427                 priv->sta_asleep_mask &= ~mask;
1428                 priv->pspoll_mask &= ~mask;
1429                 eth_zero_addr(map_link.mac_addr);
1430                 spin_unlock_bh(&priv->ps_state_lock);
1431                 reset.link_id = i + 1;
1432                 wsm_reset(priv, &reset);
1433                 spin_lock_bh(&priv->ps_state_lock);
1434             } else {
1435                 next_gc = min_t(unsigned long, next_gc, ttl);
1436             }
1437         } else if (priv->link_id_db[i].status == CW1200_LINK_RESET ||
1438                 priv->link_id_db[i].status ==
1439                 CW1200_LINK_RESET_REMAP) {
1440             int status = priv->link_id_db[i].status;
1441             priv->link_id_db[i].status =
1442                     priv->link_id_db[i].prev_status;
1443             priv->link_id_db[i].timestamp = now;
1444             reset.link_id = i + 1;
1445             spin_unlock_bh(&priv->ps_state_lock);
1446             wsm_reset(priv, &reset);
1447             if (status == CW1200_LINK_RESET_REMAP) {
1448                 memcpy(map_link.mac_addr,
1449                        priv->link_id_db[i].mac,
1450                        ETH_ALEN);
1451                 map_link.link_id = i + 1;
1452                 wsm_map_link(priv, &map_link);
1453                 next_gc = min(next_gc,
1454                         CW1200_LINK_ID_GC_TIMEOUT);
1455             }
1456             spin_lock_bh(&priv->ps_state_lock);
1457         }
1458         if (need_reset) {
1459             skb_queue_purge(&priv->link_id_db[i].rx_queue);
1460             pr_debug("[AP] STA removed, link_id: %d\n",
1461                  reset.link_id);
1462         }
1463     }
1464     spin_unlock_bh(&priv->ps_state_lock);
1465     if (next_gc != -1)
1466         queue_delayed_work(priv->workqueue,
1467                    &priv->link_id_gc_work, next_gc);
1468     wsm_unlock_tx(priv);
1469 }
[ Source navigation ] [ Diff markup ] [ Identifier search ] [ general search ]

This page was automatically generated by the 2.1.0 LXR engine.

The LXR team
Valid CSS 2.1! Valid HTML 4.01!