1 files changed, 162 insertions, 51 deletions

diff --git a/drivers/net/wireless/iwlwifi/mvm/sta.c b/drivers/net/wireless/iwlwifi/mvm/sta.c
index 3397f59cd4e4..f339ef884250 100644
--- a/drivers/net/wireless/iwlwifi/mvm/sta.c
+++ b/drivers/net/wireless/iwlwifi/mvm/sta.c
@@ -66,27 +66,27 @@
 #include "sta.h"
 #include "rs.h"
 
-static void iwl_mvm_add_sta_cmd_v6_to_v5(struct iwl_mvm_add_sta_cmd_v6 *cmd_v6,
+static void iwl_mvm_add_sta_cmd_v7_to_v5(struct iwl_mvm_add_sta_cmd_v7 *cmd_v7,
                                          struct iwl_mvm_add_sta_cmd_v5 *cmd_v5)
 {
         memset(cmd_v5, 0, sizeof(*cmd_v5));
 
-        cmd_v5->add_modify = cmd_v6->add_modify;
-        cmd_v5->tid_disable_tx = cmd_v6->tid_disable_tx;
-        cmd_v5->mac_id_n_color = cmd_v6->mac_id_n_color;
-        memcpy(cmd_v5->addr, cmd_v6->addr, ETH_ALEN);
-        cmd_v5->sta_id = cmd_v6->sta_id;
-        cmd_v5->modify_mask = cmd_v6->modify_mask;
-        cmd_v5->station_flags = cmd_v6->station_flags;
-        cmd_v5->station_flags_msk = cmd_v6->station_flags_msk;
-        cmd_v5->add_immediate_ba_tid = cmd_v6->add_immediate_ba_tid;
-        cmd_v5->remove_immediate_ba_tid = cmd_v6->remove_immediate_ba_tid;
-        cmd_v5->add_immediate_ba_ssn = cmd_v6->add_immediate_ba_ssn;
-        cmd_v5->sleep_tx_count = cmd_v6->sleep_tx_count;
-        cmd_v5->sleep_state_flags = cmd_v6->sleep_state_flags;
-        cmd_v5->assoc_id = cmd_v6->assoc_id;
-        cmd_v5->beamform_flags = cmd_v6->beamform_flags;
-        cmd_v5->tfd_queue_msk = cmd_v6->tfd_queue_msk;
+        cmd_v5->add_modify = cmd_v7->add_modify;
+        cmd_v5->tid_disable_tx = cmd_v7->tid_disable_tx;
+        cmd_v5->mac_id_n_color = cmd_v7->mac_id_n_color;
+        memcpy(cmd_v5->addr, cmd_v7->addr, ETH_ALEN);
+        cmd_v5->sta_id = cmd_v7->sta_id;
+        cmd_v5->modify_mask = cmd_v7->modify_mask;
+        cmd_v5->station_flags = cmd_v7->station_flags;
+        cmd_v5->station_flags_msk = cmd_v7->station_flags_msk;
+        cmd_v5->add_immediate_ba_tid = cmd_v7->add_immediate_ba_tid;
+        cmd_v5->remove_immediate_ba_tid = cmd_v7->remove_immediate_ba_tid;
+        cmd_v5->add_immediate_ba_ssn = cmd_v7->add_immediate_ba_ssn;
+        cmd_v5->sleep_tx_count = cmd_v7->sleep_tx_count;
+        cmd_v5->sleep_state_flags = cmd_v7->sleep_state_flags;
+        cmd_v5->assoc_id = cmd_v7->assoc_id;
+        cmd_v5->beamform_flags = cmd_v7->beamform_flags;
+        cmd_v5->tfd_queue_msk = cmd_v7->tfd_queue_msk;
 }
 
 static void
@@ -110,7 +110,7 @@ iwl_mvm_add_sta_key_to_add_sta_cmd_v5(struct iwl_mvm_add_sta_key_cmd *key_cmd,
 }
 
 static int iwl_mvm_send_add_sta_cmd_status(struct iwl_mvm *mvm,
-                                           struct iwl_mvm_add_sta_cmd_v6 *cmd,
+                                           struct iwl_mvm_add_sta_cmd_v7 *cmd,
                                            int *status)
 {
         struct iwl_mvm_add_sta_cmd_v5 cmd_v5;
@@ -119,14 +119,14 @@ static int iwl_mvm_send_add_sta_cmd_status(struct iwl_mvm *mvm,
                 return iwl_mvm_send_cmd_pdu_status(mvm, ADD_STA, sizeof(*cmd),
                                                    cmd, status);
 
-        iwl_mvm_add_sta_cmd_v6_to_v5(cmd, &cmd_v5);
+        iwl_mvm_add_sta_cmd_v7_to_v5(cmd, &cmd_v5);
 
         return iwl_mvm_send_cmd_pdu_status(mvm, ADD_STA, sizeof(cmd_v5),
                                            &cmd_v5, status);
 }
 
 static int iwl_mvm_send_add_sta_cmd(struct iwl_mvm *mvm, u32 flags,
-                                    struct iwl_mvm_add_sta_cmd_v6 *cmd)
+                                    struct iwl_mvm_add_sta_cmd_v7 *cmd)
 {
         struct iwl_mvm_add_sta_cmd_v5 cmd_v5;
 
@@ -134,7 +134,7 @@ static int iwl_mvm_send_add_sta_cmd(struct iwl_mvm *mvm, u32 flags,
                 return iwl_mvm_send_cmd_pdu(mvm, ADD_STA, flags,
                                             sizeof(*cmd), cmd);
 
-        iwl_mvm_add_sta_cmd_v6_to_v5(cmd, &cmd_v5);
+        iwl_mvm_add_sta_cmd_v7_to_v5(cmd, &cmd_v5);
 
         return iwl_mvm_send_cmd_pdu(mvm, ADD_STA, flags, sizeof(cmd_v5),
                                     &cmd_v5);
@@ -175,19 +175,30 @@ static int iwl_mvm_send_add_sta_key_cmd(struct iwl_mvm *mvm,
                                     &sta_cmd);
 }
 
-static int iwl_mvm_find_free_sta_id(struct iwl_mvm *mvm)
+static int iwl_mvm_find_free_sta_id(struct iwl_mvm *mvm,
+                                    enum nl80211_iftype iftype)
 {
         int sta_id;
+        u32 reserved_ids = 0;
 
+        BUILD_BUG_ON(IWL_MVM_STATION_COUNT > 32);
         WARN_ON_ONCE(test_bit(IWL_MVM_STATUS_IN_HW_RESTART, &mvm->status));
 
         lockdep_assert_held(&mvm->mutex);
 
+        /* d0i3/d3 assumes the AP's sta_id (of sta vif) is 0. reserve it. */
+        if (iftype != NL80211_IFTYPE_STATION)
+                reserved_ids = BIT(0);
+
         /* Don't take rcu_read_lock() since we are protected by mvm->mutex */
-        for (sta_id = 0; sta_id < IWL_MVM_STATION_COUNT; sta_id++)
+        for (sta_id = 0; sta_id < IWL_MVM_STATION_COUNT; sta_id++) {
+                if (BIT(sta_id) & reserved_ids)
+                        continue;
+
                 if (!rcu_dereference_protected(mvm->fw_id_to_mac_id[sta_id],
                                                lockdep_is_held(&mvm->mutex)))
                         return sta_id;
+        }
         return IWL_MVM_STATION_COUNT;
 }
 
@@ -196,7 +207,7 @@ int iwl_mvm_sta_send_to_fw(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
                            bool update)
 {
         struct iwl_mvm_sta *mvm_sta = (void *)sta->drv_priv;
-        struct iwl_mvm_add_sta_cmd_v6 add_sta_cmd;
+        struct iwl_mvm_add_sta_cmd_v7 add_sta_cmd;
         int ret;
         u32 status;
         u32 agg_size = 0, mpdu_dens = 0;
@@ -312,7 +323,8 @@ int iwl_mvm_add_sta(struct iwl_mvm *mvm,
         lockdep_assert_held(&mvm->mutex);
 
         if (!test_bit(IWL_MVM_STATUS_IN_HW_RESTART, &mvm->status))
-                sta_id = iwl_mvm_find_free_sta_id(mvm);
+                sta_id = iwl_mvm_find_free_sta_id(mvm,
+                                                  ieee80211_vif_type_p2p(vif));
         else
                 sta_id = mvm_sta->sta_id;
 
@@ -368,7 +380,7 @@ int iwl_mvm_update_sta(struct iwl_mvm *mvm,
 int iwl_mvm_drain_sta(struct iwl_mvm *mvm, struct iwl_mvm_sta *mvmsta,
                       bool drain)
 {
-        struct iwl_mvm_add_sta_cmd_v6 cmd = {};
+        struct iwl_mvm_add_sta_cmd_v7 cmd = {};
         int ret;
         u32 status;
 
@@ -522,6 +534,10 @@ int iwl_mvm_rm_sta(struct iwl_mvm *mvm,
 
                 /* unassoc - go ahead - remove the AP STA now */
                 mvmvif->ap_sta_id = IWL_MVM_STATION_COUNT;
+
+                /* clear d0i3_ap_sta_id if no longer relevant */
+                if (mvm->d0i3_ap_sta_id == mvm_sta->sta_id)
+                        mvm->d0i3_ap_sta_id = IWL_MVM_STATION_COUNT;
         }
 
         /*
@@ -560,10 +576,10 @@ int iwl_mvm_rm_sta_id(struct iwl_mvm *mvm,
 }
 
 int iwl_mvm_allocate_int_sta(struct iwl_mvm *mvm, struct iwl_mvm_int_sta *sta,
-                             u32 qmask)
+                             u32 qmask, enum nl80211_iftype iftype)
 {
         if (!test_bit(IWL_MVM_STATUS_IN_HW_RESTART, &mvm->status)) {
-                sta->sta_id = iwl_mvm_find_free_sta_id(mvm);
+                sta->sta_id = iwl_mvm_find_free_sta_id(mvm, iftype);
                 if (WARN_ON_ONCE(sta->sta_id == IWL_MVM_STATION_COUNT))
                         return -ENOSPC;
         }
@@ -587,13 +603,13 @@ static int iwl_mvm_add_int_sta_common(struct iwl_mvm *mvm,
                                       const u8 *addr,
                                       u16 mac_id, u16 color)
 {
-        struct iwl_mvm_add_sta_cmd_v6 cmd;
+        struct iwl_mvm_add_sta_cmd_v7 cmd;
         int ret;
         u32 status;
 
         lockdep_assert_held(&mvm->mutex);
 
-        memset(&cmd, 0, sizeof(struct iwl_mvm_add_sta_cmd_v6));
+        memset(&cmd, 0, sizeof(struct iwl_mvm_add_sta_cmd_v7));
         cmd.sta_id = sta->sta_id;
         cmd.mac_id_n_color = cpu_to_le32(FW_CMD_ID_AND_COLOR(mac_id,
                                                              color));
@@ -627,7 +643,8 @@ int iwl_mvm_add_aux_sta(struct iwl_mvm *mvm)
         lockdep_assert_held(&mvm->mutex);
 
         /* Add the aux station, but without any queues */
-        ret = iwl_mvm_allocate_int_sta(mvm, &mvm->aux_sta, 0);
+        ret = iwl_mvm_allocate_int_sta(mvm, &mvm->aux_sta, 0,
+                                       NL80211_IFTYPE_UNSPECIFIED);
         if (ret)
                 return ret;
 
@@ -699,7 +716,8 @@ int iwl_mvm_add_bcast_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
         lockdep_assert_held(&mvm->mutex);
 
         qmask = iwl_mvm_mac_get_queues_mask(mvm, vif);
-        ret = iwl_mvm_allocate_int_sta(mvm, bsta, qmask);
+        ret = iwl_mvm_allocate_int_sta(mvm, bsta, qmask,
+                                       ieee80211_vif_type_p2p(vif));
         if (ret)
                 return ret;
 
@@ -735,7 +753,7 @@ int iwl_mvm_sta_rx_agg(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
                        int tid, u16 ssn, bool start)
 {
         struct iwl_mvm_sta *mvm_sta = (void *)sta->drv_priv;
-        struct iwl_mvm_add_sta_cmd_v6 cmd = {};
+        struct iwl_mvm_add_sta_cmd_v7 cmd = {};
         int ret;
         u32 status;
 
@@ -794,7 +812,7 @@ static int iwl_mvm_sta_tx_agg(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
                               int tid, u8 queue, bool start)
 {
         struct iwl_mvm_sta *mvm_sta = (void *)sta->drv_priv;
-        struct iwl_mvm_add_sta_cmd_v6 cmd = {};
+        struct iwl_mvm_add_sta_cmd_v7 cmd = {};
         int ret;
         u32 status;
 
@@ -833,7 +851,7 @@ static int iwl_mvm_sta_tx_agg(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
         return ret;
 }
 
-static const u8 tid_to_ac[] = {
+const u8 tid_to_mac80211_ac[] = {
         IEEE80211_AC_BE,
         IEEE80211_AC_BK,
         IEEE80211_AC_BK,
@@ -844,6 +862,17 @@ static const u8 tid_to_ac[] = {
         IEEE80211_AC_VO,
 };
 
+static const u8 tid_to_ucode_ac[] = {
+        AC_BE,
+        AC_BK,
+        AC_BK,
+        AC_BE,
+        AC_VI,
+        AC_VI,
+        AC_VO,
+        AC_VO,
+};
+
 int iwl_mvm_sta_tx_agg_start(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
                              struct ieee80211_sta *sta, u16 tid, u16 *ssn)
 {
@@ -873,10 +902,18 @@ int iwl_mvm_sta_tx_agg_start(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
                 return -EIO;
         }
 
+        spin_lock_bh(&mvmsta->lock);
+
+        /* possible race condition - we entered D0i3 while starting agg */
+        if (test_bit(IWL_MVM_STATUS_IN_D0I3, &mvm->status)) {
+                spin_unlock_bh(&mvmsta->lock);
+                IWL_ERR(mvm, "Entered D0i3 while starting Tx agg\n");
+                return -EIO;
+        }
+
         /* the new tx queue is still connected to the same mac80211 queue */
-        mvm->queue_to_mac80211[txq_id] = vif->hw_queue[tid_to_ac[tid]];
+        mvm->queue_to_mac80211[txq_id] = vif->hw_queue[tid_to_mac80211_ac[tid]];
 
-        spin_lock_bh(&mvmsta->lock);
         tid_data = &mvmsta->tid_data[tid];
         tid_data->ssn = IEEE80211_SEQ_TO_SN(tid_data->seq_number);
         tid_data->txq_id = txq_id;
@@ -916,7 +953,7 @@ int iwl_mvm_sta_tx_agg_oper(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
         tid_data->ssn = 0xffff;
         spin_unlock_bh(&mvmsta->lock);
 
-        fifo = iwl_mvm_ac_to_tx_fifo[tid_to_ac[tid]];
+        fifo = iwl_mvm_ac_to_tx_fifo[tid_to_mac80211_ac[tid]];
 
         ret = iwl_mvm_sta_tx_agg(mvm, sta, tid, queue, true);
         if (ret)
@@ -1411,7 +1448,7 @@ void iwl_mvm_sta_modify_ps_wake(struct iwl_mvm *mvm,
                                 struct ieee80211_sta *sta)
 {
         struct iwl_mvm_sta *mvmsta = iwl_mvm_sta_from_mac80211(sta);
-        struct iwl_mvm_add_sta_cmd_v6 cmd = {
+        struct iwl_mvm_add_sta_cmd_v7 cmd = {
                 .add_modify = STA_MODE_MODIFY,
                 .sta_id = mvmsta->sta_id,
                 .station_flags_msk = cpu_to_le32(STA_FLG_PS),
@@ -1427,28 +1464,102 @@ void iwl_mvm_sta_modify_ps_wake(struct iwl_mvm *mvm,
 void iwl_mvm_sta_modify_sleep_tx_count(struct iwl_mvm *mvm,
                                        struct ieee80211_sta *sta,
                                        enum ieee80211_frame_release_type reason,
-                                       u16 cnt)
+                                       u16 cnt, u16 tids, bool more_data,
+                                       bool agg)
 {
-        u16 sleep_state_flags =
-                (reason == IEEE80211_FRAME_RELEASE_UAPSD) ?
-                        STA_SLEEP_STATE_UAPSD : STA_SLEEP_STATE_PS_POLL;
         struct iwl_mvm_sta *mvmsta = iwl_mvm_sta_from_mac80211(sta);
-        struct iwl_mvm_add_sta_cmd_v6 cmd = {
+        struct iwl_mvm_add_sta_cmd_v7 cmd = {
                 .add_modify = STA_MODE_MODIFY,
                 .sta_id = mvmsta->sta_id,
                 .modify_mask = STA_MODIFY_SLEEPING_STA_TX_COUNT,
                 .sleep_tx_count = cpu_to_le16(cnt),
                 .mac_id_n_color = cpu_to_le32(mvmsta->mac_id_n_color),
-                /*
-                 * Same modify mask for sleep_tx_count and sleep_state_flags so
-                 * we must set the sleep_state_flags too.
-                 */
-                .sleep_state_flags = cpu_to_le16(sleep_state_flags),
         };
-        int ret;
+        int tid, ret;
+        unsigned long _tids = tids;
+
+        /* convert TIDs to ACs - we don't support TSPEC so that's OK
+         * Note that this field is reserved and unused by firmware not
+         * supporting GO uAPSD, so it's safe to always do this.
+         */
+        for_each_set_bit(tid, &_tids, IWL_MAX_TID_COUNT)
+                cmd.awake_acs |= BIT(tid_to_ucode_ac[tid]);
+
+        /* If we're releasing frames from aggregation queues then check if the
+         * all queues combined that we're releasing frames from have
+         *  - more frames than the service period, in which case more_data
+         *    needs to be set
+         *  - fewer than 'cnt' frames, in which case we need to adjust the
+         *    firmware command (but do that unconditionally)
+         */
+        if (agg) {
+                int remaining = cnt;
+
+                spin_lock_bh(&mvmsta->lock);
+                for_each_set_bit(tid, &_tids, IWL_MAX_TID_COUNT) {
+                        struct iwl_mvm_tid_data *tid_data;
+                        u16 n_queued;
+
+                        tid_data = &mvmsta->tid_data[tid];
+                        if (WARN(tid_data->state != IWL_AGG_ON &&
+                                 tid_data->state != IWL_EMPTYING_HW_QUEUE_DELBA,
+                                 "TID %d state is %d\n",
+                                 tid, tid_data->state)) {
+                                spin_unlock_bh(&mvmsta->lock);
+                                ieee80211_sta_eosp(sta);
+                                return;
+                        }
+
+                        n_queued = iwl_mvm_tid_queued(tid_data);
+                        if (n_queued > remaining) {
+                                more_data = true;
+                                remaining = 0;
+                                break;
+                        }
+                        remaining -= n_queued;
+                }
+                spin_unlock_bh(&mvmsta->lock);
+
+                cmd.sleep_tx_count = cpu_to_le16(cnt - remaining);
+                if (WARN_ON(cnt - remaining == 0)) {
+                        ieee80211_sta_eosp(sta);
+                        return;
+                }
+        }
+
+        /* Note: this is ignored by firmware not supporting GO uAPSD */
+        if (more_data)
+                cmd.sleep_state_flags |= cpu_to_le16(STA_SLEEP_STATE_MOREDATA);
+
+        if (reason == IEEE80211_FRAME_RELEASE_PSPOLL) {
+                mvmsta->next_status_eosp = true;
+                cmd.sleep_state_flags |= cpu_to_le16(STA_SLEEP_STATE_PS_POLL);
+        } else {
+                cmd.sleep_state_flags |= cpu_to_le16(STA_SLEEP_STATE_UAPSD);
+        }
 
-        /* TODO: somehow the fw doesn't seem to take PS_POLL into account */
         ret = iwl_mvm_send_add_sta_cmd(mvm, CMD_ASYNC, &cmd);
         if (ret)
                 IWL_ERR(mvm, "Failed to send ADD_STA command (%d)\n", ret);
 }
+
+int iwl_mvm_rx_eosp_notif(struct iwl_mvm *mvm,
+                          struct iwl_rx_cmd_buffer *rxb,
+                          struct iwl_device_cmd *cmd)
+{
+        struct iwl_rx_packet *pkt = rxb_addr(rxb);
+        struct iwl_mvm_eosp_notification *notif = (void *)pkt->data;
+        struct ieee80211_sta *sta;
+        u32 sta_id = le32_to_cpu(notif->sta_id);
+
+        if (WARN_ON_ONCE(sta_id >= IWL_MVM_STATION_COUNT))
+                return 0;
+
+        rcu_read_lock();
+        sta = rcu_dereference(mvm->fw_id_to_mac_id[sta_id]);
+        if (!IS_ERR_OR_NULL(sta))
+                ieee80211_sta_eosp(sta);
+        rcu_read_unlock();
+
+        return 0;
+}