1 files changed, 17 insertions, 40 deletions

diff --git a/drivers/net/wireless/rt2x00/rt2800lib.c b/drivers/net/wireless/rt2x00/rt2800lib.c
index 0bc5ac56f283..9524564f873b 100644
--- a/drivers/net/wireless/rt2x00/rt2800lib.c
+++ b/drivers/net/wireless/rt2x00/rt2800lib.c
@@ -1381,38 +1381,6 @@ int rt2800_config_shared_key(struct rt2x00_dev *rt2x00dev,
 }
 EXPORT_SYMBOL_GPL(rt2800_config_shared_key);
 
-static inline int rt2800_find_wcid(struct rt2x00_dev *rt2x00dev)
-{
-        struct mac_wcid_entry wcid_entry;
-        int idx;
-        u32 offset;
-
-        /*
-         * Search for the first free WCID entry and return the corresponding
-         * index.
-         *
-         * Make sure the WCID starts _after_ the last possible shared key
-         * entry (>32).
-         *
-         * Since parts of the pairwise key table might be shared with
-         * the beacon frame buffers 6 & 7 we should only write into the
-         * first 222 entries.
-         */
-        for (idx = 33; idx <= 222; idx++) {
-                offset = MAC_WCID_ENTRY(idx);
-                rt2800_register_multiread(rt2x00dev, offset, &wcid_entry,
-                                          sizeof(wcid_entry));
-                if (is_broadcast_ether_addr(wcid_entry.mac))
-                        return idx;
-        }
-
-        /*
-         * Use -1 to indicate that we don't have any more space in the WCID
-         * table.
-         */
-        return -1;
-}
-
 int rt2800_config_pairwise_key(struct rt2x00_dev *rt2x00dev,
                                struct rt2x00lib_crypto *crypto,
                                struct ieee80211_key_conf *key)
@@ -1425,7 +1393,7 @@ int rt2800_config_pairwise_key(struct rt2x00_dev *rt2x00dev,
                  * Allow key configuration only for STAs that are
                  * known by the hw.
                  */
-                if (crypto->wcid < 0)
+                if (crypto->wcid > WCID_END)
                         return -ENOSPC;
                 key->hw_key_idx = crypto->wcid;
 
@@ -1455,11 +1423,13 @@ int rt2800_sta_add(struct rt2x00_dev *rt2x00dev, struct ieee80211_vif *vif,
 {
         int wcid;
         struct rt2x00_sta *sta_priv = sta_to_rt2x00_sta(sta);
+        struct rt2800_drv_data *drv_data = rt2x00dev->drv_data;
 
         /*
-         * Find next free WCID.
+         * Search for the first free WCID entry and return the corresponding
+         * index.
          */
-        wcid = rt2800_find_wcid(rt2x00dev);
+        wcid = find_first_zero_bit(drv_data->sta_ids, STA_IDS_SIZE) + WCID_START;
 
         /*
          * Store selected wcid even if it is invalid so that we can
@@ -1471,9 +1441,11 @@ int rt2800_sta_add(struct rt2x00_dev *rt2x00dev, struct ieee80211_vif *vif,
          * No space left in the device, however, we can still communicate
          * with the STA -> No error.
          */
-        if (wcid < 0)
+        if (wcid > WCID_END)
                 return 0;
 
+        __set_bit(wcid - WCID_START, drv_data->sta_ids);
+
         /*
          * Clean up WCID attributes and write STA address to the device.
          */
@@ -1487,11 +1459,16 @@ EXPORT_SYMBOL_GPL(rt2800_sta_add);
 
 int rt2800_sta_remove(struct rt2x00_dev *rt2x00dev, int wcid)
 {
+        struct rt2800_drv_data *drv_data = rt2x00dev->drv_data;
+
+        if (wcid > WCID_END)
+                return 0;
         /*
          * Remove WCID entry, no need to clean the attributes as they will
          * get renewed when the WCID is reused.
          */
         rt2800_config_wcid(rt2x00dev, NULL, wcid);
+        __clear_bit(wcid - WCID_START, drv_data->sta_ids);
 
         return 0;
 }
@@ -7968,11 +7945,11 @@ int rt2800_ampdu_action(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
         /*
          * Don't allow aggregation for stations the hardware isn't aware
          * of because tx status reports for frames to an unknown station
-         * always contain wcid=255 and thus we can't distinguish between
-         * multiple stations which leads to unwanted situations when the
-         * hw reorders frames due to aggregation.
+         * always contain wcid=WCID_END+1 and thus we can't distinguish
+         * between multiple stations which leads to unwanted situations
+         * when the hw reorders frames due to aggregation.
          */
-        if (sta_priv->wcid < 0)
+        if (sta_priv->wcid > WCID_END)
                 return 1;
 
         switch (action) {