mt76: mt7915: fix SMPS operation fail

TGn fails sending SM power save mode action frame to the AP to switch
from dynamic SMPS mode to static mode.

Reported-by: Fang Zhao <fang.zhao@mediatek.com>
Signed-off-by: Fang Zhao <fang.zhao@mediatek.com>
Signed-off-by: Ryder Lee <ryder.lee@mediatek.com>
Signed-off-by: Felix Fietkau <nbd@nbd.name>

diff --git a/drivers/net/wireless/mediatek/mt76/mt7915/mcu.c b/drivers/net/wireless/mediatek/mt76/mt7915/mcu.c
index 96e101f..e775573 100644 (file)
--- a/drivers/net/wireless/mediatek/mt76/mt7915/mcu.c
+++ b/drivers/net/wireless/mediatek/mt76/mt7915/mcu.c
@@ -1752,33 +1752,6 @@ int mt7915_mcu_sta_update_hdr_trans(struct mt7915_dev *dev,
                                     true);
 }
 
-int mt7915_mcu_add_smps(struct mt7915_dev *dev, struct ieee80211_vif *vif,
-                       struct ieee80211_sta *sta)
-{
-       struct mt7915_vif *mvif = (struct mt7915_vif *)vif->drv_priv;
-       struct mt7915_sta *msta = (struct mt7915_sta *)sta->drv_priv;
-       struct wtbl_req_hdr *wtbl_hdr;
-       struct tlv *sta_wtbl;
-       struct sk_buff *skb;
-
-       skb = mt7915_mcu_alloc_sta_req(dev, mvif, msta,
-                                      MT7915_STA_UPDATE_MAX_SIZE);
-       if (IS_ERR(skb))
-               return PTR_ERR(skb);
-
-       sta_wtbl = mt7915_mcu_add_tlv(skb, STA_REC_WTBL, sizeof(struct tlv));
-
-       wtbl_hdr = mt7915_mcu_alloc_wtbl_req(dev, msta, WTBL_SET, sta_wtbl,
-                                            &skb);
-       if (IS_ERR(wtbl_hdr))
-               return PTR_ERR(wtbl_hdr);
-
-       mt7915_mcu_wtbl_smps_tlv(skb, sta, sta_wtbl, wtbl_hdr);
-
-       return mt76_mcu_skb_send_msg(&dev->mt76, skb,
-                                    MCU_EXT_CMD(STA_REC_UPDATE), true);
-}
-
 static inline bool
 mt7915_is_ebf_supported(struct mt7915_phy *phy, struct ieee80211_vif *vif,
                        struct ieee80211_sta *sta, bool bfee)
@@ -2049,6 +2022,21 @@ mt7915_mcu_sta_bfee_tlv(struct mt7915_dev *dev, struct sk_buff *skb,
        bfee->fb_identity_matrix = (nrow == 1 && tx_ant == 2);
 }
 
+static enum mcu_mmps_mode
+mt7915_mcu_get_mmps_mode(enum ieee80211_smps_mode smps)
+{
+       switch (smps) {
+       case IEEE80211_SMPS_OFF:
+               return MCU_MMPS_DISABLE;
+       case IEEE80211_SMPS_STATIC:
+               return MCU_MMPS_STATIC;
+       case IEEE80211_SMPS_DYNAMIC:
+               return MCU_MMPS_DYNAMIC;
+       default:
+               return MCU_MMPS_DISABLE;
+       }
+}
+
 int mt7915_mcu_set_fixed_rate_ctrl(struct mt7915_dev *dev,
                                   struct ieee80211_vif *vif,
                                   struct ieee80211_sta *sta,
@@ -2076,7 +2064,11 @@ int mt7915_mcu_set_fixed_rate_ctrl(struct mt7915_dev *dev,
        case RATE_PARAM_FIXED_MCS:
        case RATE_PARAM_FIXED_GI:
        case RATE_PARAM_FIXED_HE_LTF:
-               ra->phy = *phy;
+               if (phy)
+                       ra->phy = *phy;
+               break;
+       case RATE_PARAM_MMPS_UPDATE:
+               ra->mmps_mode = mt7915_mcu_get_mmps_mode(sta->smps_mode);
                break;
        default:
                break;
@@ -2087,6 +2079,39 @@ int mt7915_mcu_set_fixed_rate_ctrl(struct mt7915_dev *dev,
                                     MCU_EXT_CMD(STA_REC_UPDATE), true);
 }
 
+int mt7915_mcu_add_smps(struct mt7915_dev *dev, struct ieee80211_vif *vif,
+                       struct ieee80211_sta *sta)
+{
+       struct mt7915_vif *mvif = (struct mt7915_vif *)vif->drv_priv;
+       struct mt7915_sta *msta = (struct mt7915_sta *)sta->drv_priv;
+       struct wtbl_req_hdr *wtbl_hdr;
+       struct tlv *sta_wtbl;
+       struct sk_buff *skb;
+       int ret;
+
+       skb = mt7915_mcu_alloc_sta_req(dev, mvif, msta,
+                                      MT7915_STA_UPDATE_MAX_SIZE);
+       if (IS_ERR(skb))
+               return PTR_ERR(skb);
+
+       sta_wtbl = mt7915_mcu_add_tlv(skb, STA_REC_WTBL, sizeof(struct tlv));
+
+       wtbl_hdr = mt7915_mcu_alloc_wtbl_req(dev, msta, WTBL_SET, sta_wtbl,
+                                            &skb);
+       if (IS_ERR(wtbl_hdr))
+               return PTR_ERR(wtbl_hdr);
+
+       mt7915_mcu_wtbl_smps_tlv(skb, sta, sta_wtbl, wtbl_hdr);
+
+       ret = mt76_mcu_skb_send_msg(&dev->mt76, skb,
+                                   MCU_EXT_CMD(STA_REC_UPDATE), true);
+       if (ret)
+               return ret;
+
+       return mt7915_mcu_set_fixed_rate_ctrl(dev, vif, sta, NULL,
+                                             RATE_PARAM_MMPS_UPDATE);
+}
+
 static int
 mt7915_mcu_add_rate_ctrl_fixed(struct mt7915_dev *dev,
                               struct ieee80211_vif *vif,

diff --git a/drivers/net/wireless/mediatek/mt76/mt7915/mcu.h b/drivers/net/wireless/mediatek/mt76/mt7915/mcu.h
index 1f5a64b..628e90d 100644 (file)
--- a/drivers/net/wireless/mediatek/mt76/mt7915/mcu.h
+++ b/drivers/net/wireless/mediatek/mt76/mt7915/mcu.h
@@ -365,6 +365,13 @@ enum {
        MCU_PHY_STATE_OFDMLQ_CNINFO,
 };
 
+enum mcu_mmps_mode {
+       MCU_MMPS_STATIC,
+       MCU_MMPS_DYNAMIC,
+       MCU_MMPS_RSV,
+       MCU_MMPS_DISABLE,
+};
+
 #define STA_TYPE_STA                   BIT(0)
 #define STA_TYPE_AP                    BIT(1)
 #define STA_TYPE_ADHOC                 BIT(2)
@@ -960,6 +967,7 @@ struct sta_rec_ra_fixed {
 
 enum {
        RATE_PARAM_FIXED = 3,
+       RATE_PARAM_MMPS_UPDATE = 5,
        RATE_PARAM_FIXED_HE_LTF = 7,
        RATE_PARAM_FIXED_MCS,
        RATE_PARAM_FIXED_GI = 11,