recipes-wifi/linux-mt76/files/patches-3.x/0138-mtk-mt76-mt7996-Fix-inconsistent-QoS-mapping-between.patch

From 0f060bd8db3e101c30617548032f00adabcd9ffa Mon Sep 17 00:00:00 2001
From: Rex Lu <rex.lu@mediatek.com>
Date: Fri, 7 Jun 2024 13:51:25 +0800
Subject: [PATCH 138/223] mtk: mt76: mt7996: Fix inconsistent QoS mapping
 between SW and HW

Fix inconsistent QoS mapping between SW and HW.
Specifically, the mapping from IP DSCP to IEEE 802.11 user priority may be customized.
Therefore, driver needs to pass the mapping to HW, so that the QoS type of traffic can be mapped in a consistent manner for both SW and HW paths.

Refactor mt7996_mcu_set_qos_map function.
1. According to fw logic, it will fill the dscp value 0~63 correspond tid.
for example:
The last set is 48-56 in the default qos map from hostapd.
And it will set tid 7. but 57-63 in original function will not fill any value,
so it will set zero to fw. Once the value of dscp is 57. mac80211 will correspond to tid 7.
and fw will correspond to tid 0.

Change-Id: Ied5b9caab869985cbbca55b401805af6787afef3
Change-Id: I6d9066c6afdfc7bb1c4523e9bc8dec40111abd25
Signed-off-by: Benjamin Lin <benjamin-jw.lin@mediatek.com>
Signed-off-by: Rex Lu <rex.lu@mediatek.com>
---
 mt76_connac_mcu.h |  1 +
 mt7996/main.c     | 31 ++++++++++++++++++++++++++
 mt7996/mcu.c      | 55 +++++++++++++++++++++++++++++++++++++++++++++++
 mt7996/mt7996.h   |  8 ++++++-
 4 files changed, 94 insertions(+), 1 deletion(-)

diff --git a/mt76_connac_mcu.h b/mt76_connac_mcu.h
index 73cf311a..1971c7bf 100644
--- a/mt76_connac_mcu.h
+++ b/mt76_connac_mcu.h
@@ -1267,6 +1267,7 @@ enum {
 	MCU_EXT_CMD_GROUP_PRE_CAL_INFO = 0xab,
 	MCU_EXT_CMD_DPD_PRE_CAL_INFO = 0xac,
 	MCU_EXT_CMD_PHY_STAT_INFO = 0xad,
+	MCU_EXT_CMD_SET_QOS_MAP = 0xb4,
 };
 
 enum {
diff --git a/mt7996/main.c b/mt7996/main.c
index 9ad4a9d6..c1c62d32 100644
--- a/mt7996/main.c
+++ b/mt7996/main.c
@@ -2369,6 +2369,9 @@ mt7996_net_fill_forward_path(struct ieee80211_hw *hw,
 
 	ctx->dev = NULL;
 
+	if (path->mtk_wdma.amsdu)
+		path->mtk_wdma.tid = mvif->qos_map[path->mtk_wdma.tid >> 2];
+
 	return 0;
 }
 
@@ -2798,6 +2801,33 @@ mt7996_event_callback(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
 	}
 }
 
+static int
+mt7996_set_qos_map(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
+		   struct cfg80211_qos_map *qos_map)
+{
+	struct mt7996_vif *mvif = (struct mt7996_vif *)vif->drv_priv;
+	struct mt7996_dev *dev = mt7996_hw_dev(hw);
+	unsigned long valid_links = vif->valid_links ?: BIT(0);
+	unsigned int link_id;
+	int ret = 0;
+
+	mutex_lock(&dev->mt76.mutex);
+	for_each_set_bit(link_id, &valid_links, IEEE80211_MLD_MAX_NUM_LINKS) {
+		struct mt7996_bss_conf *mconf =
+			mconf_dereference_protected(mvif, link_id);
+
+		if (!mconf)
+			continue;
+
+		ret = mt7996_mcu_set_qos_map(dev, mconf, qos_map);
+		if(ret)
+			break;
+	}
+	mutex_unlock(&dev->mt76.mutex);
+
+	return ret;
+}
+
 const struct ieee80211_ops mt7996_ops = {
 	.add_chanctx = ieee80211_emulate_add_chanctx,
 	.remove_chanctx = ieee80211_emulate_remove_chanctx,
@@ -2865,4 +2895,5 @@ const struct ieee80211_ops mt7996_ops = {
 	.switch_vif_chanctx = mt7996_switch_vif_chanctx,
 	.change_vif_links = mt7996_change_vif_links,
 	.change_sta_links = mt7996_change_sta_links,
+	.set_qos_map = mt7996_set_qos_map,
 };
diff --git a/mt7996/mcu.c b/mt7996/mcu.c
index 043f8d5c..edbe9cbe 100644
--- a/mt7996/mcu.c
+++ b/mt7996/mcu.c
@@ -6693,4 +6693,59 @@ int mt7996_mcu_set_csi(struct mt7996_phy *phy, u8 mode,
 		return -EINVAL;
 	}
 }
+
+int mt7996_mcu_set_qos_map(struct mt7996_dev *dev, struct mt7996_bss_conf *mconf,
+			   struct cfg80211_qos_map *usr_qos_map)
+{
+	struct {
+		u8 bss_idx;
+		u8 qos_map_enable;
+		u8 __rsv[2];
+		s8 qos_map[IP_DSCP_NUM];
+	} __packed req = {
+		.bss_idx = mconf->mt76.idx,
+		.qos_map_enable = true,
+	};
+	s8 i;
+
+	/* Default QoS map, defined in section 2.3 of RFC8325.
+	 * Three most significant bits of DSCP are used as UP.
+	 */
+	for (i = 0; i < IP_DSCP_NUM; ++i)
+		req.qos_map[i] = i >> 3;
+
+	/* Recommended QoS map, defined in section 4 of RFC8325.
+	 * Used in cfg80211_classify8021d since kernel v6.8.
+	 */
+	req.qos_map[10] = req.qos_map[12] = req.qos_map[14] = req.qos_map[16] = 0;
+	req.qos_map[18] = req.qos_map[20] = req.qos_map[22] = 3;
+	req.qos_map[24] = 4;
+	req.qos_map[40] = 5;
+	req.qos_map[44] = req.qos_map[46] = 6;
+	req.qos_map[48] = 7;
+
+	/* User-defined QoS map */
+	if (usr_qos_map) {
+		for (i = 0; i < IEEE80211_NUM_UPS; ++i) {
+			u8 low = usr_qos_map->up[i].low;
+			u8 high = usr_qos_map->up[i].high;
+
+			if (low < IP_DSCP_NUM && high < IP_DSCP_NUM && low <= high)
+				memset(req.qos_map + low, i, high - low + 1);
+		}
+
+		for (i = 0; i < usr_qos_map->num_des; ++i) {
+			u8 dscp = usr_qos_map->dscp_exception[i].dscp;
+			u8 up = usr_qos_map->dscp_exception[i].up;
+
+			if (dscp < IP_DSCP_NUM && up < IEEE80211_NUM_UPS)
+				req.qos_map[dscp] = up;
+		}
+	}
+
+	memcpy(mconf->vif->qos_map, req.qos_map, IP_DSCP_NUM);
+
+	return mt76_mcu_send_msg(&dev->mt76, MCU_WA_EXT_CMD(SET_QOS_MAP), &req,
+				 sizeof(req), false);
+}
 #endif
diff --git a/mt7996/mt7996.h b/mt7996/mt7996.h
index 3ff9cb68..d97df22f 100644
--- a/mt7996/mt7996.h
+++ b/mt7996/mt7996.h
@@ -136,6 +136,8 @@
 #define MT7996_MAX_PROBE_TIMEOUT	500
 #define MT7996_MAX_PROBE_TRIES		2
 
+#define IP_DSCP_NUM			64
+
 struct mt7996_vif;
 struct mt7996_sta;
 struct mt7996_dfs_pulse;
@@ -377,6 +379,9 @@ struct mt7996_vif {
 	void *probe[__MT_MAX_BAND];
 	unsigned long probe_send_time[__MT_MAX_BAND];
 	int probe_send_count[__MT_MAX_BAND];
+
+	/* QoS map support */
+	u8 qos_map[IP_DSCP_NUM];
 };
 
 /* crash-dump */
@@ -1337,5 +1342,6 @@ int mt7996_mcu_mlo_agc(struct mt7996_dev *dev, const void *data, int len);
 #ifdef CONFIG_NET_MEDIATEK_SOC_WED
 int mt7996_dma_rro_init(struct mt7996_dev *dev);
 #endif /* CONFIG_NET_MEDIATEK_SOC_WED */
-
+int mt7996_mcu_set_qos_map(struct mt7996_dev *dev, struct mt7996_bss_conf *mconf,
+			   struct cfg80211_qos_map *usr_qos_map);
 #endif
-- 
2.45.2