[PATCH 13/18] wifi: mac80211: add support for parsing TID to Link mapping element

[gregory.greenman@xxxxxxxxx]

From: Ayala Beker <ayala.beker@xxxxxxxxx>

Add the relevant definitions for TID to Link mapping element
according to the P802.11be_D3.0.

Signed-off-by: Ayala Beker <ayala.beker@xxxxxxxxx>
Signed-off-by: Gregory Greenman <gregory.greenman@xxxxxxxxx>
---
 include/linux/ieee80211.h  | 51 ++++++++++++++++++++++++++++++++++++++
 net/mac80211/ieee80211_i.h |  3 +++
 net/mac80211/util.c        |  8 ++++++
 3 files changed, 62 insertions(+)

diff --git a/include/linux/ieee80211.h b/include/linux/ieee80211.h
index f11b7022d9eb..6e8913dbbff6 100644
--- a/include/linux/ieee80211.h
+++ b/include/linux/ieee80211.h
@@ -1246,6 +1246,23 @@ struct ieee80211_twt_setup {
 	u8 params[];
 } __packed;
 
+#define IEEE80211_T2L_MAP_MAX_CNT			2
+#define IEEE80211_T2L_MAP_CONTROL_DIRECTION		0x03
+#define IEEE80211_T2L_MAP_CONTROL_DEF_LINK_MAP		0x04
+#define IEEE80211_T2L_MAP_CONTROL_SWITCH_TIME_PRESENT	0x08
+#define IEEE80211_T2L_MAP_CONTROL_EXPECTED_DUR_PRESENT	0x10
+#define IEEE80211_T2L_MAP_CONTROL_LINK_MAP_SIZE		0x20
+
+#define IEEE80211_T2L_MAP_DIRECTION_DOWN		0
+#define IEEE80211_T2L_MAP_DIRECTION_UP			1
+#define IEEE80211_T2L_MAP_DIRECTION_BOTH		2
+
+struct ieee80211_t2l_map_elem {
+	/* the second part of control field is in optional[] */
+	u8 control;
+	u8 optional[];
+} __packed;
+
 struct ieee80211_mgmt {
 	__le16 frame_control;
 	__le16 duration;
@@ -3618,6 +3635,7 @@ enum ieee80211_eid_ext {
 	WLAN_EID_EXT_EHT_OPERATION = 106,
 	WLAN_EID_EXT_EHT_MULTI_LINK = 107,
 	WLAN_EID_EXT_EHT_CAPABILITY = 108,
+	WLAN_EID_EXT_TID_TO_LINK_MAPPING = 109,
 	WLAN_EID_EXT_BANDWIDTH_INDICATION = 135,
 };
 
@@ -5155,6 +5173,39 @@ static inline bool ieee80211_mle_reconf_sta_prof_size_ok(const u8 *data,
 	       fixed + prof->sta_info_len - 1 <= len;
 }
 
+static inline bool ieee80211_tid_to_link_map_size_ok(const u8 *data, size_t len)
+{
+	const struct ieee80211_t2l_map_elem *t2l = (const void *)data;
+	u8 control, fixed = sizeof(*t2l), elem_len = 0;
+
+	if (len < fixed)
+		return false;
+
+	control = t2l->control;
+
+	if (control & IEEE80211_T2L_MAP_CONTROL_SWITCH_TIME_PRESENT)
+		elem_len += 2;
+	if (control & IEEE80211_T2L_MAP_CONTROL_EXPECTED_DUR_PRESENT)
+		elem_len += 3;
+
+	if (!(control & IEEE80211_T2L_MAP_CONTROL_DEF_LINK_MAP)) {
+		u8 bm_size;
+
+		elem_len += 1;
+		if (len < fixed + elem_len)
+			return false;
+
+		if (control & IEEE80211_T2L_MAP_CONTROL_LINK_MAP_SIZE)
+			bm_size = 1;
+		else
+			bm_size = 2;
+
+		elem_len += hweight8(t2l->optional[0]) * bm_size;
+	}
+
+	return len >= fixed + elem_len;
+}
+
 #define for_each_mle_subelement(_elem, _data, _len)			\
 	if (ieee80211_mle_size_ok(_data, _len))				\
 		for_each_element(_elem,					\
diff --git a/net/mac80211/ieee80211_i.h b/net/mac80211/ieee80211_i.h
index 6c56ec42dde9..fde956f8a939 100644
--- a/net/mac80211/ieee80211_i.h
+++ b/net/mac80211/ieee80211_i.h
@@ -1678,6 +1678,7 @@ struct ieee802_11_elems {
 	const struct ieee80211_multi_link_elem *ml_basic;
 	const struct ieee80211_multi_link_elem *ml_reconf;
 	const struct ieee80211_bandwidth_indication *bandwidth_indication;
+	const struct ieee80211_t2l_map_elem *t2l_map[IEEE80211_T2L_MAP_MAX_CNT];
 
 	/* length of them, respectively */
 	u8 ext_capab_len;
@@ -1711,6 +1712,8 @@ struct ieee802_11_elems {
 	/* The reconfiguration Multi-Link element in the original IEs */
 	const struct element *ml_reconf_elem;
 
+	u8 t2l_map_num;
+
 	/*
 	 * store the per station profile pointer and length in case that the
 	 * parsing also handled Multi-Link element parsing for a specific link
diff --git a/net/mac80211/util.c b/net/mac80211/util.c
index 97c5823da0eb..648d4b883d43 100644
--- a/net/mac80211/util.c
+++ b/net/mac80211/util.c
@@ -995,6 +995,14 @@ ieee80211_parse_extension_element(u32 *crc,
 			elems->bandwidth_indication = data;
 		calc_crc = true;
 		break;
+	case WLAN_EID_EXT_TID_TO_LINK_MAPPING:
+		calc_crc = true;
+		if (ieee80211_tid_to_link_map_size_ok(data, len) &&
+		    elems->t2l_map_num < ARRAY_SIZE(elems->t2l_map)) {
+			elems->t2l_map[elems->t2l_map_num] = (void *)data;
+			elems->t2l_map_num++;
+		}
+		break;
 	}
 
 	if (crc && calc_crc)
-- 
2.38.1