1 files changed, 100 insertions, 34 deletions

diff --git a/drivers/net/wireless/rtlwifi/rtl8192cu/hw.c b/drivers/net/wireless/rtlwifi/rtl8192cu/hw.c
index 3d0498e69c8c..189ba124a8c6 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192cu/hw.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192cu/hw.c
@@ -1973,26 +1973,35 @@ void rtl92cu_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
         }
 }
 
-void rtl92cu_update_hal_rate_table(struct ieee80211_hw *hw,
-                                   struct ieee80211_sta *sta,
-                                   u8 rssi_level)
+static void rtl92cu_update_hal_rate_table(struct ieee80211_hw *hw,
+                                          struct ieee80211_sta *sta)
 {
         struct rtl_priv *rtlpriv = rtl_priv(hw);
         struct rtl_phy *rtlphy = &(rtlpriv->phy);
         struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-        u32 ratr_value = (u32) mac->basic_rates;
-        u8 *mcsrate = mac->mcs;
+        struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+        u32 ratr_value;
         u8 ratr_index = 0;
         u8 nmode = mac->ht_enable;
-        u8 mimo_ps = 1;
-        u16 shortgi_rate = 0;
-        u32 tmp_ratr_value = 0;
+        u8 mimo_ps = IEEE80211_SMPS_OFF;
+        u16 shortgi_rate;
+        u32 tmp_ratr_value;
         u8 curtxbw_40mhz = mac->bw_40;
-        u8 curshortgi_40mhz = mac->sgi_40;
-        u8 curshortgi_20mhz = mac->sgi_20;
+        u8 curshortgi_40mhz = (sta->ht_cap.cap & IEEE80211_HT_CAP_SGI_40) ?
+                               1 : 0;
+        u8 curshortgi_20mhz = (sta->ht_cap.cap & IEEE80211_HT_CAP_SGI_20) ?
+                               1 : 0;
         enum wireless_mode wirelessmode = mac->mode;
 
-        ratr_value |= ((*(u16 *) (mcsrate))) << 12;
+        if (rtlhal->current_bandtype == BAND_ON_5G)
+                ratr_value = sta->supp_rates[1] << 4;
+        else
+                ratr_value = sta->supp_rates[0];
+        if (mac->opmode == NL80211_IFTYPE_ADHOC)
+                ratr_value = 0xfff;
+
+        ratr_value |= (sta->ht_cap.mcs.rx_mask[1] << 20 |
+                        sta->ht_cap.mcs.rx_mask[0] << 12);
         switch (wirelessmode) {
         case WIRELESS_MODE_B:
                 if (ratr_value & 0x0000000c)
@@ -2006,7 +2015,7 @@ void rtl92cu_update_hal_rate_table(struct ieee80211_hw *hw,
         case WIRELESS_MODE_N_24G:
         case WIRELESS_MODE_N_5G:
                 nmode = 1;
-                if (mimo_ps == 0) {
+                if (mimo_ps == IEEE80211_SMPS_STATIC) {
                         ratr_value &= 0x0007F005;
                 } else {
                         u32 ratr_mask;
@@ -2016,8 +2025,7 @@ void rtl92cu_update_hal_rate_table(struct ieee80211_hw *hw,
                                 ratr_mask = 0x000ff005;
                         else
                                 ratr_mask = 0x0f0ff005;
-                        if (curtxbw_40mhz)
-                                ratr_mask |= 0x00000010;
+
                         ratr_value &= ratr_mask;
                 }
                 break;
@@ -2026,41 +2034,74 @@ void rtl92cu_update_hal_rate_table(struct ieee80211_hw *hw,
                         ratr_value &= 0x000ff0ff;
                 else
                         ratr_value &= 0x0f0ff0ff;
+
                 break;
         }
+
         ratr_value &= 0x0FFFFFFF;
-        if (nmode && ((curtxbw_40mhz && curshortgi_40mhz) ||
-            (!curtxbw_40mhz && curshortgi_20mhz))) {
+
+        if (nmode && ((curtxbw_40mhz &&
+                         curshortgi_40mhz) || (!curtxbw_40mhz &&
+                                               curshortgi_20mhz))) {
+
                 ratr_value |= 0x10000000;
                 tmp_ratr_value = (ratr_value >> 12);
+
                 for (shortgi_rate = 15; shortgi_rate > 0; shortgi_rate--) {
                         if ((1 << shortgi_rate) & tmp_ratr_value)
                                 break;
                 }
+
                 shortgi_rate = (shortgi_rate << 12) | (shortgi_rate << 8) |
-                               (shortgi_rate << 4) | (shortgi_rate);
+                    (shortgi_rate << 4) | (shortgi_rate);
         }
+
         rtl_write_dword(rtlpriv, REG_ARFR0 + ratr_index * 4, ratr_value);
+
+        RT_TRACE(rtlpriv, COMP_RATR, DBG_DMESG, "%x\n",
+                 rtl_read_dword(rtlpriv, REG_ARFR0));
 }
 
-void rtl92cu_update_hal_rate_mask(struct ieee80211_hw *hw, u8 rssi_level)
+static void rtl92cu_update_hal_rate_mask(struct ieee80211_hw *hw,
+                                         struct ieee80211_sta *sta,
+                                         u8 rssi_level)
 {
         struct rtl_priv *rtlpriv = rtl_priv(hw);
         struct rtl_phy *rtlphy = &(rtlpriv->phy);
         struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-        u32 ratr_bitmap = (u32) mac->basic_rates;
-        u8 *p_mcsrate = mac->mcs;
-        u8 ratr_index = 0;
-        u8 curtxbw_40mhz = mac->bw_40;
-        u8 curshortgi_40mhz = mac->sgi_40;
-        u8 curshortgi_20mhz = mac->sgi_20;
-        enum wireless_mode wirelessmode = mac->mode;
+        struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+        struct rtl_sta_info *sta_entry = NULL;
+        u32 ratr_bitmap;
+        u8 ratr_index;
+        u8 curtxbw_40mhz = (sta->bandwidth >= IEEE80211_STA_RX_BW_40) ? 1 : 0;
+        u8 curshortgi_40mhz = curtxbw_40mhz &&
+                              (sta->ht_cap.cap & IEEE80211_HT_CAP_SGI_40) ?
+                                1 : 0;
+        u8 curshortgi_20mhz = (sta->ht_cap.cap & IEEE80211_HT_CAP_SGI_20) ?
+                                1 : 0;
+        enum wireless_mode wirelessmode = 0;
         bool shortgi = false;
         u8 rate_mask[5];
         u8 macid = 0;
-        u8 mimops = 1;
-
-        ratr_bitmap |= (p_mcsrate[1] << 20) | (p_mcsrate[0] << 12);
+        u8 mimo_ps = IEEE80211_SMPS_OFF;
+
+        sta_entry = (struct rtl_sta_info *) sta->drv_priv;
+        wirelessmode = sta_entry->wireless_mode;
+        if (mac->opmode == NL80211_IFTYPE_STATION ||
+            mac->opmode == NL80211_IFTYPE_MESH_POINT)
+                curtxbw_40mhz = mac->bw_40;
+        else if (mac->opmode == NL80211_IFTYPE_AP ||
+                mac->opmode == NL80211_IFTYPE_ADHOC)
+                macid = sta->aid + 1;
+
+        if (rtlhal->current_bandtype == BAND_ON_5G)
+                ratr_bitmap = sta->supp_rates[1] << 4;
+        else
+                ratr_bitmap = sta->supp_rates[0];
+        if (mac->opmode == NL80211_IFTYPE_ADHOC)
+                ratr_bitmap = 0xfff;
+        ratr_bitmap |= (sta->ht_cap.mcs.rx_mask[1] << 20 |
+                        sta->ht_cap.mcs.rx_mask[0] << 12);
         switch (wirelessmode) {
         case WIRELESS_MODE_B:
                 ratr_index = RATR_INX_WIRELESS_B;
@@ -2071,6 +2112,7 @@ void rtl92cu_update_hal_rate_mask(struct ieee80211_hw *hw, u8 rssi_level)
                 break;
         case WIRELESS_MODE_G:
                 ratr_index = RATR_INX_WIRELESS_GB;
+
                 if (rssi_level == 1)
                         ratr_bitmap &= 0x00000f00;
                 else if (rssi_level == 2)
@@ -2085,7 +2127,8 @@ void rtl92cu_update_hal_rate_mask(struct ieee80211_hw *hw, u8 rssi_level)
         case WIRELESS_MODE_N_24G:
         case WIRELESS_MODE_N_5G:
                 ratr_index = RATR_INX_WIRELESS_NGB;
-                if (mimops == 0) {
+
+                if (mimo_ps == IEEE80211_SMPS_STATIC) {
                         if (rssi_level == 1)
                                 ratr_bitmap &= 0x00070000;
                         else if (rssi_level == 2)
@@ -2128,8 +2171,10 @@ void rtl92cu_update_hal_rate_mask(struct ieee80211_hw *hw, u8 rssi_level)
                                 }
                         }
                 }
+
                 if ((curtxbw_40mhz && curshortgi_40mhz) ||
                     (!curtxbw_40mhz && curshortgi_20mhz)) {
+
                         if (macid == 0)
                                 shortgi = true;
                         else if (macid == 1)
@@ -2138,21 +2183,42 @@ void rtl92cu_update_hal_rate_mask(struct ieee80211_hw *hw, u8 rssi_level)
                 break;
         default:
                 ratr_index = RATR_INX_WIRELESS_NGB;
+
                 if (rtlphy->rf_type == RF_1T2R)
                         ratr_bitmap &= 0x000ff0ff;
                 else
                         ratr_bitmap &= 0x0f0ff0ff;
                 break;
         }
-        RT_TRACE(rtlpriv, COMP_RATR, DBG_DMESG, "ratr_bitmap :%x\n",
-                 ratr_bitmap);
-        *(u32 *)&rate_mask = ((ratr_bitmap & 0x0fffffff) |
-                                      ratr_index << 28);
+        sta_entry->ratr_index = ratr_index;
+
+        RT_TRACE(rtlpriv, COMP_RATR, DBG_DMESG,
+                 "ratr_bitmap :%x\n", ratr_bitmap);
+        *(u32 *)&rate_mask = (ratr_bitmap & 0x0fffffff) |
+                                     (ratr_index << 28);
         rate_mask[4] = macid | (shortgi ? 0x20 : 0x00) | 0x80;
         RT_TRACE(rtlpriv, COMP_RATR, DBG_DMESG,
                  "Rate_index:%x, ratr_val:%x, %5phC\n",
                  ratr_index, ratr_bitmap, rate_mask);
-        rtl92c_fill_h2c_cmd(hw, H2C_RA_MASK, 5, rate_mask);
+        memcpy(rtlpriv->rate_mask, rate_mask, 5);
+        /* rtl92c_fill_h2c_cmd() does USB I/O and will result in a
+         * "scheduled while atomic" if called directly */
+        schedule_work(&rtlpriv->works.fill_h2c_cmd);
+
+        if (macid != 0)
+                sta_entry->ratr_index = ratr_index;
+}
+
+void rtl92cu_update_hal_rate_tbl(struct ieee80211_hw *hw,
+                                 struct ieee80211_sta *sta,
+                                 u8 rssi_level)
+{
+        struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+        if (rtlpriv->dm.useramask)
+                rtl92cu_update_hal_rate_mask(hw, sta, rssi_level);
+        else
+                rtl92cu_update_hal_rate_table(hw, sta);
 }
 
 void rtl92cu_update_channel_access_setting(struct ieee80211_hw *hw)