From ccf9793e168c9e733479b0b5a3f7caca8da3ad63 Mon Sep 17 00:00:00 2001
From: Lian Chen <lian.chen@mediatek.com>
Date: Thu, 4 May 2023 15:02:49 +0800
Subject: [PATCH] mac80211 add mesh eht support

---
 include/linux/ieee80211.h  |   6 ++
 include/net/mac80211.h     |   5 ++
 net/mac80211/he.c          |  16 +++++
 net/mac80211/ht.c          |   1 +
 net/mac80211/ieee80211_i.h |   6 ++
 net/mac80211/mesh.c        |  86 +++++++++++++++++++++++-
 net/mac80211/mesh.h        |   4 ++
 net/mac80211/mesh_plink.c  |  15 ++++-
 net/mac80211/mlme.c        |   3 +
 net/mac80211/util.c        | 131 ++++++++++++++++++++++++++++++-------
 net/wireless/chan.c        |   5 ++
 11 files changed, 249 insertions(+), 29 deletions(-)

diff --git a/include/linux/ieee80211.h b/include/linux/ieee80211.h
index 0781b76..151b087 100644
--- a/include/linux/ieee80211.h
+++ b/include/linux/ieee80211.h
@@ -3986,6 +3986,12 @@ struct ieee80211_multiple_bssid_configuration {
 #define WLAN_OUI_TYPE_MICROSOFT_WPS	4
 #define WLAN_OUI_TYPE_MICROSOFT_TPC	8
 
+#define IEEE80211_EHT_OPER_CTRL_CHANWIDTH_20MHZ        0
+#define IEEE80211_EHT_OPER_CTRL_CHANWIDTH_40MHZ        1
+#define IEEE80211_EHT_OPER_CTRL_CHANWIDTH_80MHZ        2
+#define IEEE80211_EHT_OPER_CTRL_CHANWIDTH_160MHZ       3
+#define IEEE80211_EHT_OPER_CTRL_CHANWIDTH_320MHZ       4
+
 /*
  * WMM/802.11e Tspec Element
  */
diff --git a/include/net/mac80211.h b/include/net/mac80211.h
index b4f156e..5d9c2cf 100644
--- a/include/net/mac80211.h
+++ b/include/net/mac80211.h
@@ -739,6 +739,11 @@ struct ieee80211_bss_conf {
 		u32 params;
 		u16 nss_set;
 	} he_oper;
+	struct {
+		u8 params;
+	        u32 basic_mcs_nss;
+	        u8 optional[3];
+	} eht_oper;
 	struct ieee80211_he_obss_pd he_obss_pd;
 	struct cfg80211_he_bss_color he_bss_color;
 	u64 used_color_bitmap;
diff --git a/net/mac80211/he.c b/net/mac80211/he.c
index 729f261..ee6f170 100644
--- a/net/mac80211/he.c
+++ b/net/mac80211/he.c
@@ -214,6 +214,22 @@ ieee80211_he_op_ie_to_bss_conf(struct ieee80211_vif *vif,
 	vif->bss_conf.he_oper.nss_set = __le16_to_cpu(he_op_ie->he_mcs_nss_set);
 }
 
+void
+ieee80211_eht_op_ie_to_bss_conf(struct ieee80211_vif *vif,
+			const struct ieee80211_eht_operation *eht_op_ie)
+{
+	memset(&vif->bss_conf.eht_oper, 0, sizeof(vif->bss_conf.eht_oper));
+	if (!eht_op_ie)
+		return;
+
+	vif->bss_conf.eht_oper.params = (eht_op_ie->params);
+	vif->bss_conf.eht_oper.basic_mcs_nss = __le32_to_cpu(eht_op_ie->basic_mcs_nss);
+	vif->bss_conf.eht_oper.optional[0] = (eht_op_ie->optional[0]);
+	vif->bss_conf.eht_oper.optional[1] = (eht_op_ie->optional[1]);
+	vif->bss_conf.eht_oper.optional[2] = (eht_op_ie->optional[2]);
+}
+
+
 void
 ieee80211_he_spr_ie_to_bss_conf(struct ieee80211_vif *vif,
 				const struct ieee80211_he_spr *he_spr_ie_elem)
diff --git a/net/mac80211/ht.c b/net/mac80211/ht.c
index ae42e95..9bfe128 100644
--- a/net/mac80211/ht.c
+++ b/net/mac80211/ht.c
@@ -271,6 +271,7 @@ bool ieee80211_ht_cap_ie_to_sta_ht_cap(struct ieee80211_sub_if_data *sdata,
 	case NL80211_CHAN_WIDTH_80:
 	case NL80211_CHAN_WIDTH_80P80:
 	case NL80211_CHAN_WIDTH_160:
+	case NL80211_CHAN_WIDTH_320:
 		bw = ht_cap.cap & IEEE80211_HT_CAP_SUP_WIDTH_20_40 ?
 				IEEE80211_STA_RX_BW_40 : IEEE80211_STA_RX_BW_20;
 		break;
diff --git a/net/mac80211/ieee80211_i.h b/net/mac80211/ieee80211_i.h
index bf2c51e..23447eb 100644
--- a/net/mac80211/ieee80211_i.h
+++ b/net/mac80211/ieee80211_i.h
@@ -2131,6 +2131,9 @@ ieee80211_he_spr_ie_to_bss_conf(struct ieee80211_vif *vif,
 void
 ieee80211_he_op_ie_to_bss_conf(struct ieee80211_vif *vif,
 			const struct ieee80211_he_operation *he_op_ie_elem);
+void
+ieee80211_eht_op_ie_to_bss_conf(struct ieee80211_vif *vif,
+			const struct ieee80211_eht_operation *eht_op_ie_elem);
 
 /* S1G */
 void ieee80211_s1g_sta_rate_init(struct sta_info *sta);
@@ -2572,6 +2575,9 @@ u8 *ieee80211_ie_build_eht_cap(u8 *pos,
 			       const struct ieee80211_sta_eht_cap *eht_cap,
 			       u8 *end,
 			       bool for_ap);
+u8 *ieee80211_ie_build_eht_oper(u8 *pos, struct cfg80211_chan_def *chandef,
+			       const struct ieee80211_sta_eht_cap *eht_cap);
+
 
 void
 ieee80211_eht_cap_ie_to_sta_eht_cap(struct ieee80211_sub_if_data *sdata,
diff --git a/net/mac80211/mesh.c b/net/mac80211/mesh.c
index 6b94cf2..c2f7bd3 100644
--- a/net/mac80211/mesh.c
+++ b/net/mac80211/mesh.c
@@ -105,7 +105,7 @@ bool mesh_matches_local(struct ieee80211_sub_if_data *sdata,
 	ieee80211_chandef_vht_oper(&sdata->local->hw, vht_cap_info,
 				   ie->vht_operation, ie->ht_operation,
 				   &sta_chan_def);
-	ieee80211_chandef_he_6ghz_oper(sdata, ie->he_operation, NULL,
+	ieee80211_chandef_he_6ghz_oper(sdata, ie->he_operation, ie->eht_operation,
 				       &sta_chan_def);
 
 	if (!cfg80211_chandef_compatible(&sdata->vif.bss_conf.chandef,
@@ -639,6 +639,67 @@ int mesh_add_he_6ghz_cap_ie(struct ieee80211_sub_if_data *sdata,
 	return 0;
 }
 
+int mesh_add_eht_cap_ie(struct ieee80211_sub_if_data *sdata,
+			struct sk_buff *skb, u8 ie_len)
+{
+	const struct ieee80211_sta_he_cap *he_cap;
+	const struct ieee80211_sta_eht_cap *eht_cap;
+	struct ieee80211_supported_band *sband;
+	u8 *pos;
+
+	sband = ieee80211_get_sband(sdata);
+	if (!sband)
+		return -EINVAL;
+
+	he_cap = ieee80211_get_he_iftype_cap(sband, NL80211_IFTYPE_MESH_POINT);
+	eht_cap = ieee80211_get_eht_iftype_cap(sband, NL80211_IFTYPE_MESH_POINT);
+
+	if (!he_cap || !eht_cap ||
+	    sdata->vif.bss_conf.chandef.width == NL80211_CHAN_WIDTH_20_NOHT ||
+	    sdata->vif.bss_conf.chandef.width == NL80211_CHAN_WIDTH_5 ||
+	    sdata->vif.bss_conf.chandef.width == NL80211_CHAN_WIDTH_10)
+		return 0;
+
+	if (skb_tailroom(skb) < ie_len)
+		return -ENOMEM;
+
+	pos = skb_put(skb, ie_len);
+	ieee80211_ie_build_eht_cap(pos, he_cap, eht_cap, pos + ie_len, false);
+
+	return 0;
+}
+
+int mesh_add_eht_oper_ie(struct ieee80211_sub_if_data *sdata,
+			 struct sk_buff *skb)
+{
+	const struct ieee80211_sta_eht_cap *eht_cap;
+	struct ieee80211_supported_band *sband;
+	u32 len;
+	u8 *pos;
+
+	sband = ieee80211_get_sband(sdata);
+	if (!sband)
+		return -EINVAL;
+
+	eht_cap = ieee80211_get_eht_iftype_cap(sband, NL80211_IFTYPE_MESH_POINT);
+	if (!eht_cap ||
+	    sdata->vif.bss_conf.chandef.width == NL80211_CHAN_WIDTH_20_NOHT ||
+	    sdata->vif.bss_conf.chandef.width == NL80211_CHAN_WIDTH_5 ||
+	    sdata->vif.bss_conf.chandef.width == NL80211_CHAN_WIDTH_10)
+		return 0;
+
+	len = 2 + 1 + sizeof(struct ieee80211_eht_operation) +
+		      sizeof(struct ieee80211_eht_operation_info);
+
+	if (skb_tailroom(skb) < len)
+		return -ENOMEM;
+
+	pos = skb_put(skb, len);
+	ieee80211_ie_build_eht_oper(pos, &sdata->vif.bss_conf.chandef, eht_cap);
+
+	return 0;
+}
+
 static void ieee80211_mesh_path_timer(struct timer_list *t)
 {
 	struct ieee80211_sub_if_data *sdata =
@@ -676,6 +737,7 @@ ieee80211_mesh_update_bss_params(struct ieee80211_sub_if_data *sdata,
 	struct ieee80211_supported_band *sband;
 	const struct element *cap;
 	const struct ieee80211_he_operation *he_oper = NULL;
+	const struct ieee80211_eht_operation *eht_oper = NULL;
 
 	sband = ieee80211_get_sband(sdata);
 	if (!sband)
@@ -697,6 +759,18 @@ ieee80211_mesh_update_bss_params(struct ieee80211_sub_if_data *sdata,
 	if (he_oper)
 		sdata->vif.bss_conf.he_oper.params =
 			__le32_to_cpu(he_oper->he_oper_params);
+
+	if (!ieee80211_get_eht_iftype_cap(sband, NL80211_IFTYPE_MESH_POINT))
+		return;
+
+	sdata->vif.bss_conf.eht_support = true;
+
+	cap = cfg80211_find_ext_elem(WLAN_EID_EXT_EHT_OPERATION, ie, ie_len);
+	if (cap && ieee80211_eht_oper_size_ok((u8 *)cap, cap->datalen))
+		eht_oper = (void *)(cap->data + 1);
+
+	if (eht_oper)
+		ieee80211_eht_op_ie_to_bss_conf(&sdata->vif, eht_oper);
 }
 
 bool ieee80211_mesh_xmit_fast(struct ieee80211_sub_if_data *sdata,
@@ -903,7 +977,7 @@ ieee80211_mesh_build_beacon(struct ieee80211_if_mesh *ifmsh)
 	struct ieee80211_chanctx_conf *chanctx_conf;
 	struct mesh_csa_settings *csa;
 	enum nl80211_band band;
-	u8 ie_len_he_cap;
+	u8 ie_len_he_cap, ie_len_eht_cap;
 	u8 *pos;
 	struct ieee80211_sub_if_data *sdata;
 	int hdr_len = offsetofend(struct ieee80211_mgmt, u.beacon);
@@ -916,6 +990,8 @@ ieee80211_mesh_build_beacon(struct ieee80211_if_mesh *ifmsh)
 
 	ie_len_he_cap = ieee80211_ie_len_he_cap(sdata,
 						NL80211_IFTYPE_MESH_POINT);
+	ie_len_eht_cap = ieee80211_ie_len_eht_cap(sdata,
+						NL80211_IFTYPE_MESH_POINT);
 	head_len = hdr_len +
 		   2 + /* NULL SSID */
 		   /* Channel Switch Announcement */
@@ -939,6 +1015,9 @@ ieee80211_mesh_build_beacon(struct ieee80211_if_mesh *ifmsh)
 		   2 + 1 + sizeof(struct ieee80211_he_operation) +
 			   sizeof(struct ieee80211_he_6ghz_oper) +
 		   2 + 1 + sizeof(struct ieee80211_he_6ghz_capa) +
+		   ie_len_eht_cap +
+		   2 + 1 + sizeof(struct ieee80211_eht_operation) +
+		   	 + sizeof(struct ieee80211_eht_operation_info) +
 		   ifmsh->ie_len;
 
 	bcn = kzalloc(sizeof(*bcn) + head_len + tail_len, GFP_KERNEL);
@@ -1019,6 +1098,7 @@ ieee80211_mesh_build_beacon(struct ieee80211_if_mesh *ifmsh)
 		case NL80211_CHAN_WIDTH_80:
 		case NL80211_CHAN_WIDTH_80P80:
 		case NL80211_CHAN_WIDTH_160:
+		case NL80211_CHAN_WIDTH_320:
 			/* Channel Switch Wrapper + Wide Bandwidth CSA IE */
 			ie_len = 2 + 2 +
 				 sizeof(struct ieee80211_wide_bw_chansw_ie);
@@ -1059,6 +1139,8 @@ ieee80211_mesh_build_beacon(struct ieee80211_if_mesh *ifmsh)
 	    mesh_add_he_cap_ie(sdata, skb, ie_len_he_cap) ||
 	    mesh_add_he_oper_ie(sdata, skb) ||
 	    mesh_add_he_6ghz_cap_ie(sdata, skb) ||
+	    mesh_add_eht_cap_ie(sdata, skb, ie_len_eht_cap) ||
+	    mesh_add_eht_oper_ie(sdata, skb) ||
 	    mesh_add_vendor_ies(sdata, skb))
 		goto out_free;
 
diff --git a/net/mac80211/mesh.h b/net/mac80211/mesh.h
index ab6747d..79b5045 100644
--- a/net/mac80211/mesh.h
+++ b/net/mac80211/mesh.h
@@ -234,6 +234,10 @@ int mesh_add_he_oper_ie(struct ieee80211_sub_if_data *sdata,
 			struct sk_buff *skb);
 int mesh_add_he_6ghz_cap_ie(struct ieee80211_sub_if_data *sdata,
 			    struct sk_buff *skb);
+int mesh_add_eht_cap_ie(struct ieee80211_sub_if_data *sdata,
+			struct sk_buff *skb, u8 ie_len);
+int mesh_add_eht_oper_ie(struct ieee80211_sub_if_data *sdata,
+			 struct sk_buff *skb);
 void mesh_rmc_free(struct ieee80211_sub_if_data *sdata);
 int mesh_rmc_init(struct ieee80211_sub_if_data *sdata);
 void ieee80211s_init(void);
diff --git a/net/mac80211/mesh_plink.c b/net/mac80211/mesh_plink.c
index ddfe510..0956727 100644
--- a/net/mac80211/mesh_plink.c
+++ b/net/mac80211/mesh_plink.c
@@ -219,12 +219,14 @@ static int mesh_plink_frame_tx(struct ieee80211_sub_if_data *sdata,
 	bool include_plid = false;
 	u16 peering_proto = 0;
 	u8 *pos, ie_len = 4;
-	u8 ie_len_he_cap;
+	u8 ie_len_he_cap, ie_len_eht_cap;
 	int hdr_len = offsetofend(struct ieee80211_mgmt, u.action.u.self_prot);
 	int err = -ENOMEM;
 
 	ie_len_he_cap = ieee80211_ie_len_he_cap(sdata,
 						NL80211_IFTYPE_MESH_POINT);
+	ie_len_eht_cap = ieee80211_ie_len_eht_cap(sdata,
+						NL80211_IFTYPE_MESH_POINT);
 	skb = dev_alloc_skb(local->tx_headroom +
 			    hdr_len +
 			    2 + /* capability info */
@@ -241,6 +243,9 @@ static int mesh_plink_frame_tx(struct ieee80211_sub_if_data *sdata,
 			    2 + 1 + sizeof(struct ieee80211_he_operation) +
 				    sizeof(struct ieee80211_he_6ghz_oper) +
 			    2 + 1 + sizeof(struct ieee80211_he_6ghz_capa) +
+			    ie_len_eht_cap +
+			    2 + 1 + sizeof(struct ieee80211_eht_operation) +
+				    sizeof(struct ieee80211_eht_operation_info) +
 			    2 + 8 + /* peering IE */
 			    sdata->u.mesh.ie_len);
 	if (!skb)
@@ -332,7 +337,9 @@ static int mesh_plink_frame_tx(struct ieee80211_sub_if_data *sdata,
 		    mesh_add_vht_oper_ie(sdata, skb) ||
 		    mesh_add_he_cap_ie(sdata, skb, ie_len_he_cap) ||
 		    mesh_add_he_oper_ie(sdata, skb) ||
-		    mesh_add_he_6ghz_cap_ie(sdata, skb))
+		    mesh_add_he_6ghz_cap_ie(sdata, skb) ||
+		    mesh_add_eht_cap_ie(sdata, skb, ie_len_eht_cap) ||
+		    mesh_add_eht_oper_ie(sdata, skb))
 			goto free;
 	}
 
@@ -450,6 +457,10 @@ static void mesh_sta_info_init(struct ieee80211_sub_if_data *sdata,
 					  elems->he_cap_len,
 					  elems->he_6ghz_capa,
 					  &sta->deflink);
+	ieee80211_eht_cap_ie_to_sta_eht_cap(sdata, sband, elems->he_cap,
+					  elems->he_cap_len,
+					  elems->eht_cap, elems->eht_cap_len,
+					  &sta->deflink);
 
 	if (bw != sta->sta.deflink.bandwidth)
 		changed |= IEEE80211_RC_BW_CHANGED;
diff --git a/net/mac80211/mlme.c b/net/mac80211/mlme.c
index 458609c..28e872b 100644
--- a/net/mac80211/mlme.c
+++ b/net/mac80211/mlme.c
@@ -557,6 +557,9 @@ static int ieee80211_config_bw(struct ieee80211_link_data *link,
 	if (link->u.mgd.conn_flags & IEEE80211_CONN_DISABLE_160MHZ &&
 	    chandef.width == NL80211_CHAN_WIDTH_160)
 		flags |= ieee80211_chandef_downgrade(&chandef);
+	if (link->u.mgd.conn_flags & IEEE80211_CONN_DISABLE_320MHZ &&
+	    chandef.width == NL80211_CHAN_WIDTH_320)
+		flags |= ieee80211_chandef_downgrade(&chandef);
 	if (link->u.mgd.conn_flags & IEEE80211_CONN_DISABLE_40MHZ &&
 	    chandef.width > NL80211_CHAN_WIDTH_20)
 		flags |= ieee80211_chandef_downgrade(&chandef);
diff --git a/net/mac80211/util.c b/net/mac80211/util.c
index f6a2561..2e5bab3 100644
--- a/net/mac80211/util.c
+++ b/net/mac80211/util.c
@@ -3133,6 +3133,7 @@ u8 *ieee80211_ie_build_ht_oper(u8 *pos, struct ieee80211_sta_ht_cap *ht_cap,
 	ht_oper->primary_chan = ieee80211_frequency_to_channel(
 					chandef->chan->center_freq);
 	switch (chandef->width) {
+	case NL80211_CHAN_WIDTH_320:
 	case NL80211_CHAN_WIDTH_160:
 	case NL80211_CHAN_WIDTH_80P80:
 	case NL80211_CHAN_WIDTH_80:
@@ -3142,10 +3143,6 @@ u8 *ieee80211_ie_build_ht_oper(u8 *pos, struct ieee80211_sta_ht_cap *ht_cap,
 		else
 			ht_oper->ht_param = IEEE80211_HT_PARAM_CHA_SEC_BELOW;
 		break;
-	case NL80211_CHAN_WIDTH_320:
-		/* HT information element should not be included on 6GHz */
-		WARN_ON(1);
-		return pos;
 	default:
 		ht_oper->ht_param = IEEE80211_HT_PARAM_CHA_SEC_NONE;
 		break;
@@ -3206,19 +3203,31 @@ u8 *ieee80211_ie_build_vht_oper(u8 *pos, struct ieee80211_sta_vht_cap *vht_cap,
 				const struct cfg80211_chan_def *chandef)
 {
 	struct ieee80211_vht_operation *vht_oper;
+	struct cfg80211_chan_def tmp_chandef;
+
+	cfg80211_chandef_create(&tmp_chandef, chandef->chan, NL80211_CHAN_NO_HT);
+	tmp_chandef.center_freq1 = chandef->center_freq1;
+	tmp_chandef.center_freq2 = chandef->center_freq2;
+	tmp_chandef.width = chandef->width;
 
 	*pos++ = WLAN_EID_VHT_OPERATION;
 	*pos++ = sizeof(struct ieee80211_vht_operation);
 	vht_oper = (struct ieee80211_vht_operation *)pos;
 	vht_oper->center_freq_seg0_idx = ieee80211_frequency_to_channel(
-							chandef->center_freq1);
-	if (chandef->center_freq2)
+							tmp_chandef.center_freq1);
+	if (tmp_chandef.center_freq2)
 		vht_oper->center_freq_seg1_idx =
-			ieee80211_frequency_to_channel(chandef->center_freq2);
+			ieee80211_frequency_to_channel(tmp_chandef.center_freq2);
 	else
 		vht_oper->center_freq_seg1_idx = 0x00;
 
-	switch (chandef->width) {
+	switch (tmp_chandef.width) {
+	case NL80211_CHAN_WIDTH_320:
+		/* Downgrade EHT 320 MHz BW to 160 MHz for VHT & set new center_freq1 */
+		ieee80211_chandef_downgrade(&tmp_chandef);
+		vht_oper->center_freq_seg0_idx =
+			ieee80211_frequency_to_channel(tmp_chandef.center_freq1);
+		fallthrough;
 	case NL80211_CHAN_WIDTH_160:
 		/*
 		 * Convert 160 MHz channel width to new style as interop
@@ -3226,7 +3235,7 @@ u8 *ieee80211_ie_build_vht_oper(u8 *pos, struct ieee80211_sta_vht_cap *vht_cap,
 		 */
 		vht_oper->chan_width = IEEE80211_VHT_CHANWIDTH_80MHZ;
 		vht_oper->center_freq_seg1_idx = vht_oper->center_freq_seg0_idx;
-		if (chandef->chan->center_freq < chandef->center_freq1)
+		if (tmp_chandef.chan->center_freq < tmp_chandef.center_freq1)
 			vht_oper->center_freq_seg0_idx -= 8;
 		else
 			vht_oper->center_freq_seg0_idx += 8;
@@ -3241,10 +3250,6 @@ u8 *ieee80211_ie_build_vht_oper(u8 *pos, struct ieee80211_sta_vht_cap *vht_cap,
 	case NL80211_CHAN_WIDTH_80:
 		vht_oper->chan_width = IEEE80211_VHT_CHANWIDTH_80MHZ;
 		break;
-	case NL80211_CHAN_WIDTH_320:
-		/* VHT information element should not be included on 6GHz */
-		WARN_ON(1);
-		return pos;
 	default:
 		vht_oper->chan_width = IEEE80211_VHT_CHANWIDTH_USE_HT;
 		break;
@@ -3262,6 +3267,7 @@ u8 *ieee80211_ie_build_he_oper(u8 *pos, struct cfg80211_chan_def *chandef)
 	struct ieee80211_he_6ghz_oper *he_6ghz_op;
 	u32 he_oper_params;
 	u8 ie_len = 1 + sizeof(struct ieee80211_he_operation);
+	struct cfg80211_chan_def tmp_chandef;
 
 	if (chandef->chan->band == NL80211_BAND_6GHZ)
 		ie_len += sizeof(struct ieee80211_he_6ghz_oper);
@@ -3291,27 +3297,30 @@ u8 *ieee80211_ie_build_he_oper(u8 *pos, struct cfg80211_chan_def *chandef)
 	if (chandef->chan->band != NL80211_BAND_6GHZ)
 		goto out;
 
+	cfg80211_chandef_create(&tmp_chandef, chandef->chan, NL80211_CHAN_NO_HT);
+	tmp_chandef.center_freq1 = chandef->center_freq1;
+	tmp_chandef.center_freq2 = chandef->center_freq2;
+	tmp_chandef.width = chandef->width;
 	/* TODO add VHT operational */
 	he_6ghz_op = (struct ieee80211_he_6ghz_oper *)pos;
 	he_6ghz_op->minrate = 6; /* 6 Mbps */
 	he_6ghz_op->primary =
-		ieee80211_frequency_to_channel(chandef->chan->center_freq);
+		ieee80211_frequency_to_channel(tmp_chandef.chan->center_freq);
 	he_6ghz_op->ccfs0 =
-		ieee80211_frequency_to_channel(chandef->center_freq1);
-	if (chandef->center_freq2)
+		ieee80211_frequency_to_channel(tmp_chandef.center_freq1);
+	if (tmp_chandef.center_freq2)
 		he_6ghz_op->ccfs1 =
-			ieee80211_frequency_to_channel(chandef->center_freq2);
+			ieee80211_frequency_to_channel(tmp_chandef.center_freq2);
 	else
 		he_6ghz_op->ccfs1 = 0;
 
-	switch (chandef->width) {
+	switch (tmp_chandef.width) {
 	case NL80211_CHAN_WIDTH_320:
-		/*
-		 * TODO: mesh operation is not defined over 6GHz 320 MHz
-		 * channels.
-		 */
-		WARN_ON(1);
-		break;
+		/* Downgrade EHT 320 MHz BW to 160 MHz for HE & set new center_freq1 */
+		ieee80211_chandef_downgrade(&tmp_chandef);
+		he_6ghz_op->ccfs0 =
+			ieee80211_frequency_to_channel(tmp_chandef.center_freq1);
+		fallthrough;
 	case NL80211_CHAN_WIDTH_160:
 		/* Convert 160 MHz channel width to new style as interop
 		 * workaround.
@@ -3319,7 +3328,7 @@ u8 *ieee80211_ie_build_he_oper(u8 *pos, struct cfg80211_chan_def *chandef)
 		he_6ghz_op->control =
 			IEEE80211_HE_6GHZ_OPER_CTRL_CHANWIDTH_160MHZ;
 		he_6ghz_op->ccfs1 = he_6ghz_op->ccfs0;
-		if (chandef->chan->center_freq < chandef->center_freq1)
+		if (tmp_chandef.chan->center_freq < tmp_chandef.center_freq1)
 			he_6ghz_op->ccfs0 -= 8;
 		else
 			he_6ghz_op->ccfs0 += 8;
@@ -3348,6 +3357,78 @@ out:
 	return pos;
 }
 
+u8 *ieee80211_ie_build_eht_oper(u8 *pos, struct cfg80211_chan_def *chandef,
+				const struct ieee80211_sta_eht_cap *eht_cap)
+
+{
+	const struct ieee80211_eht_mcs_nss_supp_20mhz_only *eht_mcs_nss =
+					&eht_cap->eht_mcs_nss_supp.only_20mhz;
+	struct ieee80211_eht_operation *eht_oper;
+	struct ieee80211_eht_operation_info *eht_oper_info;
+	u8 eht_oper_len = offsetof(struct ieee80211_eht_operation, optional);
+	u8 eht_oper_info_len =
+		offsetof(struct ieee80211_eht_operation_info, optional);
+	u8 chan_width = 0;
+
+	*pos++ = WLAN_EID_EXTENSION;
+	*pos++ = 1 + eht_oper_len + eht_oper_info_len;
+	*pos++ = WLAN_EID_EXT_EHT_OPERATION;
+
+	eht_oper = (struct ieee80211_eht_operation *)pos;
+
+	memcpy(&eht_oper->basic_mcs_nss, eht_mcs_nss, sizeof(*eht_mcs_nss));
+	eht_oper->params |= IEEE80211_EHT_OPER_INFO_PRESENT;
+	pos += eht_oper_len;
+
+	eht_oper_info =
+		(struct ieee80211_eht_operation_info *)eht_oper->optional;
+
+	eht_oper_info->ccfs0 =
+		ieee80211_frequency_to_channel(chandef->center_freq1);
+	if (chandef->center_freq2)
+		eht_oper_info->ccfs1 =
+			ieee80211_frequency_to_channel(chandef->center_freq2);
+	else
+		eht_oper_info->ccfs1 = 0;
+
+	switch (chandef->width) {
+	case NL80211_CHAN_WIDTH_320:
+		chan_width = IEEE80211_EHT_OPER_CHAN_WIDTH_320MHZ;
+		eht_oper_info->ccfs1 = eht_oper_info->ccfs0;
+		if (chandef->chan->center_freq < chandef->center_freq1)
+			eht_oper_info->ccfs0 -= 16;
+		else
+			eht_oper_info->ccfs0 += 16;
+		break;
+	case NL80211_CHAN_WIDTH_160:
+		eht_oper_info->ccfs1 = eht_oper_info->ccfs0;
+		if (chandef->chan->center_freq < chandef->center_freq1)
+			eht_oper_info->ccfs0 -= 8;
+		else
+			eht_oper_info->ccfs0 += 8;
+		fallthrough;
+	case NL80211_CHAN_WIDTH_80P80:
+		chan_width = IEEE80211_EHT_OPER_CHAN_WIDTH_160MHZ;
+		break;
+	case NL80211_CHAN_WIDTH_80:
+		chan_width = IEEE80211_EHT_OPER_CHAN_WIDTH_80MHZ;
+		break;
+	case NL80211_CHAN_WIDTH_40:
+		chan_width = IEEE80211_EHT_OPER_CHAN_WIDTH_40MHZ;
+		break;
+	default:
+		chan_width = IEEE80211_EHT_OPER_CHAN_WIDTH_20MHZ;
+		break;
+	}
+	eht_oper_info->control = chan_width;
+	pos += eht_oper_info_len;
+
+	/* TODO: eht_oper_info->optional */
+
+	return pos;
+}
+
+
 bool ieee80211_chandef_ht_oper(const struct ieee80211_ht_operation *ht_oper,
 			       struct cfg80211_chan_def *chandef)
 {
diff --git a/net/wireless/chan.c b/net/wireless/chan.c
index d5ed976..fcae66b 100644
--- a/net/wireless/chan.c
+++ b/net/wireless/chan.c
@@ -256,6 +256,11 @@ bool cfg80211_chandef_valid(const struct cfg80211_chan_def *chandef)
 		    chandef->center_freq2 - chandef->center_freq1 == 80)
 			return false;
 		break;
+	case NL80211_CHAN_WIDTH_160:
+	case NL80211_CHAN_WIDTH_320:
+		/* while center_freq2 is not mandatory, donot reject when
+		 * this is specified */
+		break;
 	default:
 		if (chandef->center_freq2)
 			return false;
-- 
2.18.0

