1 files changed, 105 insertions, 3 deletions

diff --git a/net/mac80211/cfg.c b/net/mac80211/cfg.c
index ac28af74a414..4a3d5a414a25 100644
--- a/net/mac80211/cfg.c
+++ b/net/mac80211/cfg.c
@@ -1059,6 +1059,7 @@ static int ieee80211_stop_ap(struct wiphy *wiphy, struct net_device *dev)
         /* abort any running channel switch */
         sdata->vif.csa_active = false;
         cancel_work_sync(&sdata->csa_finalize_work);
+        cancel_work_sync(&sdata->u.ap.request_smps_work);
 
         /* turn off carrier for this interface and dependent VLANs */
         list_for_each_entry(vlan, &sdata->u.ap.vlans, u.vlan.list)
@@ -1553,6 +1554,20 @@ static int ieee80211_change_station(struct wiphy *wiphy,
 
         mutex_unlock(&local->sta_mtx);
 
+        if ((sdata->vif.type == NL80211_IFTYPE_AP ||
+             sdata->vif.type == NL80211_IFTYPE_AP_VLAN) &&
+            sta->known_smps_mode != sta->sdata->bss->req_smps &&
+            test_sta_flag(sta, WLAN_STA_AUTHORIZED) &&
+            sta_info_tx_streams(sta) != 1) {
+                ht_dbg(sta->sdata,
+                       "%pM just authorized and MIMO capable - update SMPS\n",
+                       sta->sta.addr);
+                ieee80211_send_smps_action(sta->sdata,
+                        sta->sdata->bss->req_smps,
+                        sta->sta.addr,
+                        sta->sdata->vif.bss_conf.bssid);
+        }
+
         if (sdata->vif.type == NL80211_IFTYPE_STATION &&
             params->sta_flags_mask & BIT(NL80211_STA_FLAG_AUTHORIZED)) {
                 ieee80211_recalc_ps(local, -1);
@@ -2337,8 +2352,92 @@ static int ieee80211_testmode_dump(struct wiphy *wiphy,
 }
 #endif
 
-int __ieee80211_request_smps(struct ieee80211_sub_if_data *sdata,
-                             enum ieee80211_smps_mode smps_mode)
+int __ieee80211_request_smps_ap(struct ieee80211_sub_if_data *sdata,
+                                enum ieee80211_smps_mode smps_mode)
+{
+        struct sta_info *sta;
+        enum ieee80211_smps_mode old_req;
+        int i;
+
+        if (WARN_ON_ONCE(sdata->vif.type != NL80211_IFTYPE_AP))
+                return -EINVAL;
+
+        if (sdata->vif.bss_conf.chandef.width == NL80211_CHAN_WIDTH_20_NOHT)
+                return 0;
+
+        old_req = sdata->u.ap.req_smps;
+        sdata->u.ap.req_smps = smps_mode;
+
+        /* AUTOMATIC doesn't mean much for AP - don't allow it */
+        if (old_req == smps_mode ||
+            smps_mode == IEEE80211_SMPS_AUTOMATIC)
+                return 0;
+
+         /* If no associated stations, there's no need to do anything */
+        if (!atomic_read(&sdata->u.ap.num_mcast_sta)) {
+                sdata->smps_mode = smps_mode;
+                ieee80211_queue_work(&sdata->local->hw, &sdata->recalc_smps);
+                return 0;
+        }
+
+        ht_dbg(sdata,
+               "SMSP %d requested in AP mode, sending Action frame to %d stations\n",
+               smps_mode, atomic_read(&sdata->u.ap.num_mcast_sta));
+
+        mutex_lock(&sdata->local->sta_mtx);
+        for (i = 0; i < STA_HASH_SIZE; i++) {
+                for (sta = rcu_dereference_protected(sdata->local->sta_hash[i],
+                                lockdep_is_held(&sdata->local->sta_mtx));
+                     sta;
+                     sta = rcu_dereference_protected(sta->hnext,
+                                lockdep_is_held(&sdata->local->sta_mtx))) {
+                        /*
+                         * Only stations associated to our AP and
+                         * associated VLANs
+                         */
+                        if (sta->sdata->bss != &sdata->u.ap)
+                                continue;
+
+                        /* This station doesn't support MIMO - skip it */
+                        if (sta_info_tx_streams(sta) == 1)
+                                continue;
+
+                        /*
+                         * Don't wake up a STA just to send the action frame
+                         * unless we are getting more restrictive.
+                         */
+                        if (test_sta_flag(sta, WLAN_STA_PS_STA) &&
+                            !ieee80211_smps_is_restrictive(sta->known_smps_mode,
+                                                           smps_mode)) {
+                                ht_dbg(sdata,
+                                       "Won't send SMPS to sleeping STA %pM\n",
+                                       sta->sta.addr);
+                                continue;
+                        }
+
+                        /*
+                         * If the STA is not authorized, wait until it gets
+                         * authorized and the action frame will be sent then.
+                         */
+                        if (!test_sta_flag(sta, WLAN_STA_AUTHORIZED))
+                                continue;
+
+                        ht_dbg(sdata, "Sending SMPS to %pM\n", sta->sta.addr);
+                        ieee80211_send_smps_action(sdata, smps_mode,
+                                                   sta->sta.addr,
+                                                   sdata->vif.bss_conf.bssid);
+                }
+        }
+        mutex_unlock(&sdata->local->sta_mtx);
+
+        sdata->smps_mode = smps_mode;
+        ieee80211_queue_work(&sdata->local->hw, &sdata->recalc_smps);
+
+        return 0;
+}
+
+int __ieee80211_request_smps_mgd(struct ieee80211_sub_if_data *sdata,
+                                 enum ieee80211_smps_mode smps_mode)
 {
         const u8 *ap;
         enum ieee80211_smps_mode old_req;
@@ -2346,6 +2445,9 @@ int __ieee80211_request_smps(struct ieee80211_sub_if_data *sdata,
 
         lockdep_assert_held(&sdata->wdev.mtx);
 
+        if (WARN_ON_ONCE(sdata->vif.type != NL80211_IFTYPE_STATION))
+                return -EINVAL;
+
         old_req = sdata->u.mgd.req_smps;
         sdata->u.mgd.req_smps = smps_mode;
 
@@ -2402,7 +2504,7 @@ static int ieee80211_set_power_mgmt(struct wiphy *wiphy, struct net_device *dev,
 
         /* no change, but if automatic follow powersave */
         sdata_lock(sdata);
-        __ieee80211_request_smps(sdata, sdata->u.mgd.req_smps);
+        __ieee80211_request_smps_mgd(sdata, sdata->u.mgd.req_smps);
         sdata_unlock(sdata);
 
         if (local->hw.flags & IEEE80211_HW_SUPPORTS_DYNAMIC_PS)