1 files changed, 72 insertions, 0 deletions

diff --git a/net/mac80211/sta_info.c b/net/mac80211/sta_info.c
index aeb967a0aeed..1eb66e26e49d 100644
--- a/net/mac80211/sta_info.c
+++ b/net/mac80211/sta_info.c
@@ -385,6 +385,30 @@ struct sta_info *sta_info_alloc(struct ieee80211_sub_if_data *sdata,
                 sta->last_seq_ctrl[i] = cpu_to_le16(USHRT_MAX);
 
         sta->sta.smps_mode = IEEE80211_SMPS_OFF;
+        if (sdata->vif.type == NL80211_IFTYPE_AP ||
+            sdata->vif.type == NL80211_IFTYPE_AP_VLAN) {
+                struct ieee80211_supported_band *sband =
+                        local->hw.wiphy->bands[ieee80211_get_sdata_band(sdata)];
+                u8 smps = (sband->ht_cap.cap & IEEE80211_HT_CAP_SM_PS) >>
+                                IEEE80211_HT_CAP_SM_PS_SHIFT;
+                /*
+                 * Assume that hostapd advertises our caps in the beacon and
+                 * this is the known_smps_mode for a station that just assciated
+                 */
+                switch (smps) {
+                case WLAN_HT_SMPS_CONTROL_DISABLED:
+                        sta->known_smps_mode = IEEE80211_SMPS_OFF;
+                        break;
+                case WLAN_HT_SMPS_CONTROL_STATIC:
+                        sta->known_smps_mode = IEEE80211_SMPS_STATIC;
+                        break;
+                case WLAN_HT_SMPS_CONTROL_DYNAMIC:
+                        sta->known_smps_mode = IEEE80211_SMPS_DYNAMIC;
+                        break;
+                default:
+                        WARN_ON(1);
+                }
+        }
 
         sta_dbg(sdata, "Allocated STA %pM\n", sta->sta.addr);
 
@@ -1069,6 +1093,19 @@ void ieee80211_sta_ps_deliver_wakeup(struct sta_info *sta)
 
         ieee80211_add_pending_skbs_fn(local, &pending, clear_sta_ps_flags, sta);
 
+        /* This station just woke up and isn't aware of our SMPS state */
+        if (!ieee80211_smps_is_restrictive(sta->known_smps_mode,
+                                           sdata->smps_mode) &&
+            sta->known_smps_mode != sdata->bss->req_smps &&
+            sta_info_tx_streams(sta) != 1) {
+                ht_dbg(sdata,
+                       "%pM just woke up and MIMO capable - update SMPS\n",
+                       sta->sta.addr);
+                ieee80211_send_smps_action(sdata, sdata->bss->req_smps,
+                                           sta->sta.addr,
+                                           sdata->vif.bss_conf.bssid);
+        }
+
         local->total_ps_buffered -= buffered;
 
         sta_info_recalc_tim(sta);
@@ -1520,3 +1557,38 @@ int sta_info_move_state(struct sta_info *sta,
 
         return 0;
 }
+
+u8 sta_info_tx_streams(struct sta_info *sta)
+{
+        struct ieee80211_sta_ht_cap *ht_cap = &sta->sta.ht_cap;
+        u8 rx_streams;
+
+        if (!sta->sta.ht_cap.ht_supported)
+                return 1;
+
+        if (sta->sta.vht_cap.vht_supported) {
+                int i;
+                u16 tx_mcs_map =
+                        le16_to_cpu(sta->sta.vht_cap.vht_mcs.tx_mcs_map);
+
+                for (i = 7; i >= 0; i--)
+                        if ((tx_mcs_map & (0x3 << (i * 2))) !=
+                            IEEE80211_VHT_MCS_NOT_SUPPORTED)
+                                return i + 1;
+        }
+
+        if (ht_cap->mcs.rx_mask[3])
+                rx_streams = 4;
+        else if (ht_cap->mcs.rx_mask[2])
+                rx_streams = 3;
+        else if (ht_cap->mcs.rx_mask[1])
+                rx_streams = 2;
+        else
+                rx_streams = 1;
+
+        if (!(ht_cap->mcs.tx_params & IEEE80211_HT_MCS_TX_RX_DIFF))
+                return rx_streams;
+
+        return ((ht_cap->mcs.tx_params & IEEE80211_HT_MCS_TX_MAX_STREAMS_MASK)
+                        >> IEEE80211_HT_MCS_TX_MAX_STREAMS_SHIFT) + 1;
+}