[PATCH] mt76: mt7915: fix SMPS operation fail

[Ryder Lee]

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 at mediatek.com>
Signed-off-by: Fang Zhao <fang.zhao at mediatek.com>
Signed-off-by: Ryder Lee <ryder.lee at mediatek.com>
---
 drivers/net/wireless/mediatek/mt76/mt7915/mac.c | 5 ++++-
 drivers/net/wireless/mediatek/mt76/mt7915/mcu.c | 6 +++++-
 drivers/net/wireless/mediatek/mt76/mt7915/mcu.h | 1 +
 3 files changed, 10 insertions(+), 2 deletions(-)

diff --git a/drivers/net/wireless/mediatek/mt76/mt7915/mac.c b/drivers/net/wireless/mediatek/mt76/mt7915/mac.c
index 79081abf3329..5021c8fd2650 100644
--- a/drivers/net/wireless/mediatek/mt76/mt7915/mac.c
+++ b/drivers/net/wireless/mediatek/mt76/mt7915/mac.c
@@ -2135,8 +2135,11 @@ void mt7915_mac_sta_rc_work(struct work_struct *work)
 			       IEEE80211_RC_BW_CHANGED))
 			mt7915_mcu_add_rate_ctrl(dev, vif, sta, true);
 
-		if (changed & IEEE80211_RC_SMPS_CHANGED)
+		if (changed & IEEE80211_RC_SMPS_CHANGED) {
 			mt7915_mcu_add_smps(dev, vif, sta);
+			mt7915_mcu_set_fixed_rate_ctrl(dev, vif, sta, NULL,
+						       RATE_PARAM_MMPS_UPDATE);
+		}
 
 		spin_lock_bh(&dev->sta_poll_lock);
 	}
diff --git a/drivers/net/wireless/mediatek/mt76/mt7915/mcu.c b/drivers/net/wireless/mediatek/mt76/mt7915/mcu.c
index 3a496046fe21..92166679055f 100644
--- a/drivers/net/wireless/mediatek/mt76/mt7915/mcu.c
+++ b/drivers/net/wireless/mediatek/mt76/mt7915/mcu.c
@@ -2077,7 +2077,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 = (sta->smps_mode == IEEE80211_SMPS_DYNAMIC);
 		break;
 	default:
 		break;
diff --git a/drivers/net/wireless/mediatek/mt76/mt7915/mcu.h b/drivers/net/wireless/mediatek/mt76/mt7915/mcu.h
index e8501234f686..11728454b92c 100644
--- a/drivers/net/wireless/mediatek/mt76/mt7915/mcu.h
+++ b/drivers/net/wireless/mediatek/mt76/mt7915/mcu.h
@@ -960,6 +960,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,
-- 
2.29.2