~hc/RK356X_SDK_RELEASE.git

..	..	@@ -1,3 +1,4 @@
	1	+// SPDX-License-Identifier: GPL-2.0-only
1	2	/*
2	3	* Copyright 2002-2005, Instant802 Networks, Inc.
3	4	* Copyright 2005-2006, Devicescape Software, Inc.
..	..	@@ -5,11 +6,7 @@
5	6	* Copyright 2007 Johannes Berg <johannes@sipsolutions.net>
6	7	* Copyright 2013-2014 Intel Mobile Communications GmbH
7	8	* Copyright (C) 2015-2017 Intel Deutschland GmbH
8		- * Copyright (C) 2018-2019 Intel Corporation
9		- *
10		- * This program is free software; you can redistribute it and/or modify
11		- * it under the terms of the GNU General Public License version 2 as
12		- * published by the Free Software Foundation.
	9	+ * Copyright (C) 2018-2020 Intel Corporation
13	10	*
14	11	* utilities for mac80211
15	12	*/
..	..	@@ -42,12 +39,63 @@
42	39	struct ieee80211_hw wiphy_to_ieee80211_hw(struct wiphy wiphy)
43	40	{
44	41	struct ieee80211_local *local;
45		- BUG_ON(!wiphy);
46	42
47	43	local = wiphy_priv(wiphy);
48	44	return &local->hw;
49	45	}
50	46	EXPORT_SYMBOL(wiphy_to_ieee80211_hw);
	47	+
	48	+u8 ieee80211_get_bssid(struct ieee80211_hdr hdr, size_t len,
	49	+ enum nl80211_iftype type)
	50	+{
	51	+ __le16 fc = hdr->frame_control;
	52	+
	53	+ if (ieee80211_is_data(fc)) {
	54	+ if (len < 24) /* drop incorrect hdr len (data) */
	55	+ return NULL;
	56	+
	57	+ if (ieee80211_has_a4(fc))
	58	+ return NULL;
	59	+ if (ieee80211_has_tods(fc))
	60	+ return hdr->addr1;
	61	+ if (ieee80211_has_fromds(fc))
	62	+ return hdr->addr2;
	63	+
	64	+ return hdr->addr3;
	65	+ }
	66	+
	67	+ if (ieee80211_is_s1g_beacon(fc)) {
	68	+ struct ieee80211_ext ext = (void ) hdr;
	69	+
	70	+ return ext->u.s1g_beacon.sa;
	71	+ }
	72	+
	73	+ if (ieee80211_is_mgmt(fc)) {
	74	+ if (len < 24) /* drop incorrect hdr len (mgmt) */
	75	+ return NULL;
	76	+ return hdr->addr3;
	77	+ }
	78	+
	79	+ if (ieee80211_is_ctl(fc)) {
	80	+ if (ieee80211_is_pspoll(fc))
	81	+ return hdr->addr1;
	82	+
	83	+ if (ieee80211_is_back_req(fc)) {
	84	+ switch (type) {
	85	+ case NL80211_IFTYPE_STATION:
	86	+ return hdr->addr2;
	87	+ case NL80211_IFTYPE_AP:
	88	+ case NL80211_IFTYPE_AP_VLAN:
	89	+ return hdr->addr1;
	90	+ default:
	91	+ break; /* fall through to the return */
	92	+ }
	93	+ }
	94	+ }
	95	+
	96	+ return NULL;
	97	+}
	98	+EXPORT_SYMBOL(ieee80211_get_bssid);
51	99
52	100	void ieee80211_tx_set_protected(struct ieee80211_tx_data *tx)
53	101	{
..	..	@@ -240,6 +288,114 @@
240	288	}
241	289	EXPORT_SYMBOL(ieee80211_ctstoself_duration);
242	290
	291	+static void __ieee80211_wake_txqs(struct ieee80211_sub_if_data *sdata, int ac)
	292	+{
	293	+ struct ieee80211_local *local = sdata->local;
	294	+ struct ieee80211_vif *vif = &sdata->vif;
	295	+ struct fq *fq = &local->fq;
	296	+ struct ps_data *ps = NULL;
	297	+ struct txq_info *txqi;
	298	+ struct sta_info *sta;
	299	+ int i;
	300	+
	301	+ local_bh_disable();
	302	+ spin_lock(&fq->lock);
	303	+
	304	+ if (sdata->vif.type == NL80211_IFTYPE_AP)
	305	+ ps = &sdata->bss->ps;
	306	+
	307	+ sdata->vif.txqs_stopped[ac] = false;
	308	+
	309	+ list_for_each_entry_rcu(sta, &local->sta_list, list) {
	310	+ if (sdata != sta->sdata)
	311	+ continue;
	312	+
	313	+ for (i = 0; i < ARRAY_SIZE(sta->sta.txq); i++) {
	314	+ struct ieee80211_txq *txq = sta->sta.txq[i];
	315	+
	316	+ if (!txq)
	317	+ continue;
	318	+
	319	+ txqi = to_txq_info(txq);
	320	+
	321	+ if (ac != txq->ac)
	322	+ continue;
	323	+
	324	+ if (!test_and_clear_bit(IEEE80211_TXQ_STOP_NETIF_TX,
	325	+ &txqi->flags))
	326	+ continue;
	327	+
	328	+ spin_unlock(&fq->lock);
	329	+ drv_wake_tx_queue(local, txqi);
	330	+ spin_lock(&fq->lock);
	331	+ }
	332	+ }
	333	+
	334	+ if (!vif->txq)
	335	+ goto out;
	336	+
	337	+ txqi = to_txq_info(vif->txq);
	338	+
	339	+ if (!test_and_clear_bit(IEEE80211_TXQ_STOP_NETIF_TX, &txqi->flags) \|\|
	340	+ (ps && atomic_read(&ps->num_sta_ps)) \|\| ac != vif->txq->ac)
	341	+ goto out;
	342	+
	343	+ spin_unlock(&fq->lock);
	344	+
	345	+ drv_wake_tx_queue(local, txqi);
	346	+ local_bh_enable();
	347	+ return;
	348	+out:
	349	+ spin_unlock(&fq->lock);
	350	+ local_bh_enable();
	351	+}
	352	+
	353	+static void
	354	+__releases(&local->queue_stop_reason_lock)
	355	+__acquires(&local->queue_stop_reason_lock)
	356	+_ieee80211_wake_txqs(struct ieee80211_local local, unsigned long flags)
	357	+{
	358	+ struct ieee80211_sub_if_data *sdata;
	359	+ int n_acs = IEEE80211_NUM_ACS;
	360	+ int i;
	361	+
	362	+ rcu_read_lock();
	363	+
	364	+ if (local->hw.queues < IEEE80211_NUM_ACS)
	365	+ n_acs = 1;
	366	+
	367	+ for (i = 0; i < local->hw.queues; i++) {
	368	+ if (local->queue_stop_reasons[i])
	369	+ continue;
	370	+
	371	+ spin_unlock_irqrestore(&local->queue_stop_reason_lock, *flags);
	372	+ list_for_each_entry_rcu(sdata, &local->interfaces, list) {
	373	+ int ac;
	374	+
	375	+ for (ac = 0; ac < n_acs; ac++) {
	376	+ int ac_queue = sdata->vif.hw_queue[ac];
	377	+
	378	+ if (ac_queue == i \|\|
	379	+ sdata->vif.cab_queue == i)
	380	+ __ieee80211_wake_txqs(sdata, ac);
	381	+ }
	382	+ }
	383	+ spin_lock_irqsave(&local->queue_stop_reason_lock, *flags);
	384	+ }
	385	+
	386	+ rcu_read_unlock();
	387	+}
	388	+
	389	+void ieee80211_wake_txqs(unsigned long data)
	390	+{
	391	+ struct ieee80211_local local = (struct ieee80211_local )data;
	392	+ unsigned long flags;
	393	+
	394	+ spin_lock_irqsave(&local->queue_stop_reason_lock, flags);
	395	+ _ieee80211_wake_txqs(local, &flags);
	396	+ spin_unlock_irqrestore(&local->queue_stop_reason_lock, flags);
	397	+}
	398	+
243	399	void ieee80211_propagate_queue_wake(struct ieee80211_local *local, int queue)
244	400	{
245	401	struct ieee80211_sub_if_data *sdata;
..	..	@@ -275,7 +431,8 @@
275	431
276	432	static void __ieee80211_wake_queue(struct ieee80211_hw *hw, int queue,
277	433	enum queue_stop_reason reason,
278		- bool refcounted)
	434	+ bool refcounted,
	435	+ unsigned long *flags)
279	436	{
280	437	struct ieee80211_local *local = hw_to_local(hw);
281	438
..	..	@@ -308,6 +465,20 @@
308	465	rcu_read_unlock();
309	466	} else
310	467	tasklet_schedule(&local->tx_pending_tasklet);
	468	+
	469	+ /*
	470	+ * Calling _ieee80211_wake_txqs here can be a problem because it may
	471	+ * release queue_stop_reason_lock which has been taken by
	472	+ * __ieee80211_wake_queue's caller. It is certainly not very nice to
	473	+ * release someone's lock, but it is fine because all the callers of
	474	+ * __ieee80211_wake_queue call it right before releasing the lock.
	475	+ */
	476	+ if (local->ops->wake_tx_queue) {
	477	+ if (reason == IEEE80211_QUEUE_STOP_REASON_DRIVER)
	478	+ tasklet_schedule(&local->wake_txqs_tasklet);
	479	+ else
	480	+ _ieee80211_wake_txqs(local, flags);
	481	+ }
311	482	}
312	483
313	484	void ieee80211_wake_queue_by_reason(struct ieee80211_hw *hw, int queue,
..	..	@@ -318,7 +489,7 @@
318	489	unsigned long flags;
319	490
320	491	spin_lock_irqsave(&local->queue_stop_reason_lock, flags);
321		- __ieee80211_wake_queue(hw, queue, reason, refcounted);
	492	+ __ieee80211_wake_queue(hw, queue, reason, refcounted, &flags);
322	493	spin_unlock_irqrestore(&local->queue_stop_reason_lock, flags);
323	494	}
324	495
..	..	@@ -351,9 +522,6 @@
351	522	if (__test_and_set_bit(reason, &local->queue_stop_reasons[queue]))
352	523	return;
353	524
354		- if (local->ops->wake_tx_queue)
355		- return;
356		-
357	525	if (local->hw.queues < IEEE80211_NUM_ACS)
358	526	n_acs = 1;
359	527
..	..	@@ -366,8 +534,15 @@
366	534
367	535	for (ac = 0; ac < n_acs; ac++) {
368	536	if (sdata->vif.hw_queue[ac] == queue \|\|
369		- sdata->vif.cab_queue == queue)
370		- netif_stop_subqueue(sdata->dev, ac);
	537	+ sdata->vif.cab_queue == queue) {
	538	+ if (!local->ops->wake_tx_queue) {
	539	+ netif_stop_subqueue(sdata->dev, ac);
	540	+ continue;
	541	+ }
	542	+ spin_lock(&local->fq.lock);
	543	+ sdata->vif.txqs_stopped[ac] = true;
	544	+ spin_unlock(&local->fq.lock);
	545	+ }
371	546	}
372	547	}
373	548	rcu_read_unlock();
..	..	@@ -411,7 +586,7 @@
411	586	false);
412	587	__skb_queue_tail(&local->pending[queue], skb);
413	588	__ieee80211_wake_queue(hw, queue, IEEE80211_QUEUE_STOP_REASON_SKB_ADD,
414		- false);
	589	+ false, &flags);
415	590	spin_unlock_irqrestore(&local->queue_stop_reason_lock, flags);
416	591	}
417	592
..	..	@@ -444,7 +619,7 @@
444	619	for (i = 0; i < hw->queues; i++)
445	620	__ieee80211_wake_queue(hw, i,
446	621	IEEE80211_QUEUE_STOP_REASON_SKB_ADD,
447		- false);
	622	+ false, &flags);
448	623	spin_unlock_irqrestore(&local->queue_stop_reason_lock, flags);
449	624	}
450	625
..	..	@@ -502,7 +677,7 @@
502	677	spin_lock_irqsave(&local->queue_stop_reason_lock, flags);
503	678
504	679	for_each_set_bit(i, &queues, hw->queues)
505		- __ieee80211_wake_queue(hw, i, reason, refcounted);
	680	+ __ieee80211_wake_queue(hw, i, reason, refcounted, &flags);
506	681
507	682	spin_unlock_irqrestore(&local->queue_stop_reason_lock, flags);
508	683	}
..	..	@@ -609,6 +784,9 @@
609	784	}
610	785	if (!(iter_flags & IEEE80211_IFACE_ITER_RESUME_ALL) &&
611	786	active_only && !(sdata->flags & IEEE80211_SDATA_IN_DRIVER))
	787	+ continue;
	788	+ if ((iter_flags & IEEE80211_IFACE_SKIP_SDATA_NOT_IN_DRIVER) &&
	789	+ !(sdata->flags & IEEE80211_SDATA_IN_DRIVER))
612	790	continue;
613	791	if (ieee80211_sdata_running(sdata) \|\| !active_only)
614	792	iterator(data, sdata->vif.addr,
..	..	@@ -767,33 +945,87 @@
767	945	}
768	946	EXPORT_SYMBOL(ieee80211_queue_delayed_work);
769	947
770		-u32 ieee802_11_parse_elems_crc(const u8 *start, size_t len, bool action,
771		- struct ieee802_11_elems *elems,
772		- u64 filter, u32 crc)
	948	+static void ieee80211_parse_extension_element(u32 *crc,
	949	+ const struct element *elem,
	950	+ struct ieee802_11_elems *elems)
773	951	{
774		- size_t left = len;
775		- const u8 *pos = start;
	952	+ const void *data = elem->data + 1;
	953	+ u8 len;
	954	+
	955	+ if (!elem->datalen)
	956	+ return;
	957	+
	958	+ len = elem->datalen - 1;
	959	+
	960	+ switch (elem->data[0]) {
	961	+ case WLAN_EID_EXT_HE_MU_EDCA:
	962	+ if (len >= sizeof(*elems->mu_edca_param_set)) {
	963	+ elems->mu_edca_param_set = data;
	964	+ if (crc)
	965	+ crc = crc32_be(crc, (void *)elem,
	966	+ elem->datalen + 2);
	967	+ }
	968	+ break;
	969	+ case WLAN_EID_EXT_HE_CAPABILITY:
	970	+ elems->he_cap = data;
	971	+ elems->he_cap_len = len;
	972	+ break;
	973	+ case WLAN_EID_EXT_HE_OPERATION:
	974	+ if (len >= sizeof(*elems->he_operation) &&
	975	+ len >= ieee80211_he_oper_size(data) - 1) {
	976	+ if (crc)
	977	+ crc = crc32_be(crc, (void *)elem,
	978	+ elem->datalen + 2);
	979	+ elems->he_operation = data;
	980	+ }
	981	+ break;
	982	+ case WLAN_EID_EXT_UORA:
	983	+ if (len >= 1)
	984	+ elems->uora_element = data;
	985	+ break;
	986	+ case WLAN_EID_EXT_MAX_CHANNEL_SWITCH_TIME:
	987	+ if (len == 3)
	988	+ elems->max_channel_switch_time = data;
	989	+ break;
	990	+ case WLAN_EID_EXT_MULTIPLE_BSSID_CONFIGURATION:
	991	+ if (len >= sizeof(*elems->mbssid_config_ie))
	992	+ elems->mbssid_config_ie = data;
	993	+ break;
	994	+ case WLAN_EID_EXT_HE_SPR:
	995	+ if (len >= sizeof(*elems->he_spr) &&
	996	+ len >= ieee80211_he_spr_size(data))
	997	+ elems->he_spr = data;
	998	+ break;
	999	+ case WLAN_EID_EXT_HE_6GHZ_CAPA:
	1000	+ if (len >= sizeof(*elems->he_6ghz_capa))
	1001	+ elems->he_6ghz_capa = data;
	1002	+ break;
	1003	+ }
	1004	+}
	1005	+
	1006	+static u32
	1007	+_ieee802_11_parse_elems_crc(const u8 *start, size_t len, bool action,
	1008	+ struct ieee802_11_elems *elems,
	1009	+ u64 filter, u32 crc,
	1010	+ const struct element *check_inherit)
	1011	+{
	1012	+ const struct element *elem;
776	1013	bool calc_crc = filter != 0;
777	1014	DECLARE_BITMAP(seen_elems, 256);
778	1015	const u8 *ie;
779	1016
780	1017	bitmap_zero(seen_elems, 256);
781		- memset(elems, 0, sizeof(*elems));
782		- elems->ie_start = start;
783		- elems->total_len = len;
784	1018
785		- while (left >= 2) {
786		- u8 id, elen;
	1019	+ for_each_element(elem, start, len) {
787	1020	bool elem_parse_failed;
	1021	+ u8 id = elem->id;
	1022	+ u8 elen = elem->datalen;
	1023	+ const u8 *pos = elem->data;
788	1024
789		- id = *pos++;
790		- elen = *pos++;
791		- left -= 2;
792		-
793		- if (elen > left) {
794		- elems->parse_error = true;
795		- break;
796		- }
	1025	+ if (check_inherit &&
	1026	+ !cfg80211_is_element_inherited(elem,
	1027	+ check_inherit))
	1028	+ continue;
797	1029
798	1030	switch (id) {
799	1031	case WLAN_EID_SSID:
..	..	@@ -830,14 +1062,18 @@
830	1062	case WLAN_EID_CHAN_SWITCH_TIMING:
831	1063	case WLAN_EID_LINK_ID:
832	1064	case WLAN_EID_BSS_MAX_IDLE_PERIOD:
	1065	+ case WLAN_EID_RSNX:
	1066	+ case WLAN_EID_S1G_BCN_COMPAT:
	1067	+ case WLAN_EID_S1G_CAPABILITIES:
	1068	+ case WLAN_EID_S1G_OPERATION:
	1069	+ case WLAN_EID_AID_RESPONSE:
	1070	+ case WLAN_EID_S1G_SHORT_BCN_INTERVAL:
833	1071	/*
834	1072	* not listing WLAN_EID_CHANNEL_SWITCH_WRAPPER -- it seems possible
835	1073	* that if the content gets bigger it might be needed more than once
836	1074	*/
837	1075	if (test_bit(id, seen_elems)) {
838	1076	elems->parse_error = true;
839		- left -= elen;
840		- pos += elen;
841	1077	continue;
842	1078	}
843	1079	break;
..	..	@@ -850,14 +1086,14 @@
850	1086
851	1087	switch (id) {
852	1088	case WLAN_EID_LINK_ID:
853		- if (elen + 2 != sizeof(struct ieee80211_tdls_lnkie)) {
	1089	+ if (elen + 2 < sizeof(struct ieee80211_tdls_lnkie)) {
854	1090	elem_parse_failed = true;
855	1091	break;
856	1092	}
857	1093	elems->lnk_id = (void *)(pos - 2);
858	1094	break;
859	1095	case WLAN_EID_CHAN_SWITCH_TIMING:
860		- if (elen != sizeof(struct ieee80211_ch_switch_timing)) {
	1096	+ if (elen < sizeof(struct ieee80211_ch_switch_timing)) {
861	1097	elem_parse_failed = true;
862	1098	break;
863	1099	}
..	..	@@ -887,10 +1123,6 @@
887	1123	elems->tim_len = elen;
888	1124	} else
889	1125	elem_parse_failed = true;
890		- break;
891		- case WLAN_EID_CHALLENGE:
892		- elems->challenge = pos;
893		- elems->challenge_len = elen;
894	1126	break;
895	1127	case WLAN_EID_VENDOR_SPECIFIC:
896	1128	if (elen >= 4 && pos[0] == 0x00 && pos[1] == 0x50 &&
..	..	@@ -1020,7 +1252,7 @@
1020	1252	elems->sec_chan_offs = (void *)pos;
1021	1253	break;
1022	1254	case WLAN_EID_CHAN_SWITCH_PARAM:
1023		- if (elen !=
	1255	+ if (elen <
1024	1256	sizeof(*elems->mesh_chansw_params_ie)) {
1025	1257	elem_parse_failed = true;
1026	1258	break;
..	..	@@ -1029,7 +1261,7 @@
1029	1261	break;
1030	1262	case WLAN_EID_WIDE_BW_CHANNEL_SWITCH:
1031	1263	if (!action \|\|
1032		- elen != sizeof(*elems->wide_bw_chansw_ie)) {
	1264	+ elen < sizeof(*elems->wide_bw_chansw_ie)) {
1033	1265	elem_parse_failed = true;
1034	1266	break;
1035	1267	}
..	..	@@ -1048,7 +1280,7 @@
1048	1280	ie = cfg80211_find_ie(WLAN_EID_WIDE_BW_CHANNEL_SWITCH,
1049	1281	pos, elen);
1050	1282	if (ie) {
1051		- if (ie[1] == sizeof(*elems->wide_bw_chansw_ie))
	1283	+ if (ie[1] >= sizeof(*elems->wide_bw_chansw_ie))
1052	1284	elems->wide_bw_chansw_ie =
1053	1285	(void *)(ie + 2);
1054	1286	else
..	..	@@ -1091,6 +1323,13 @@
1091	1323
1092	1324	elems->cisco_dtpc_elem = pos;
1093	1325	break;
	1326	+ case WLAN_EID_ADDBA_EXT:
	1327	+ if (elen < sizeof(struct ieee80211_addba_ext_ie)) {
	1328	+ elem_parse_failed = true;
	1329	+ break;
	1330	+ }
	1331	+ elems->addba_ext_ie = (void *)pos;
	1332	+ break;
1094	1333	case WLAN_EID_TIMEOUT_INTERVAL:
1095	1334	if (elen >= sizeof(struct ieee80211_timeout_interval_ie))
1096	1335	elems->timeout_int = (void *)pos;
..	..	@@ -1101,22 +1340,38 @@
1101	1340	if (elen >= sizeof(*elems->max_idle_period_ie))
1102	1341	elems->max_idle_period_ie = (void *)pos;
1103	1342	break;
	1343	+ case WLAN_EID_RSNX:
	1344	+ elems->rsnx = pos;
	1345	+ elems->rsnx_len = elen;
	1346	+ break;
1104	1347	case WLAN_EID_EXTENSION:
1105		- if (!elen)
1106		- break;
1107		- if (pos[0] == WLAN_EID_EXT_HE_MU_EDCA &&
1108		- elen >= (sizeof(*elems->mu_edca_param_set) + 1)) {
1109		- elems->mu_edca_param_set = (void *)&pos[1];
1110		- } else if (pos[0] == WLAN_EID_EXT_HE_CAPABILITY) {
1111		- elems->he_cap = (void *)&pos[1];
1112		- elems->he_cap_len = elen - 1;
1113		- } else if (pos[0] == WLAN_EID_EXT_HE_OPERATION &&
1114		- elen >= sizeof(*elems->he_operation) &&
1115		- elen >= ieee80211_he_oper_size(&pos[1])) {
1116		- elems->he_operation = (void *)&pos[1];
1117		- } else if (pos[0] == WLAN_EID_EXT_UORA && elen >= 1) {
1118		- elems->uora_element = (void *)&pos[1];
1119		- }
	1348	+ ieee80211_parse_extension_element(calc_crc ?
	1349	+ &crc : NULL,
	1350	+ elem, elems);
	1351	+ break;
	1352	+ case WLAN_EID_S1G_CAPABILITIES:
	1353	+ if (elen >= sizeof(*elems->s1g_capab))
	1354	+ elems->s1g_capab = (void *)pos;
	1355	+ else
	1356	+ elem_parse_failed = true;
	1357	+ break;
	1358	+ case WLAN_EID_S1G_OPERATION:
	1359	+ if (elen == sizeof(*elems->s1g_oper))
	1360	+ elems->s1g_oper = (void *)pos;
	1361	+ else
	1362	+ elem_parse_failed = true;
	1363	+ break;
	1364	+ case WLAN_EID_S1G_BCN_COMPAT:
	1365	+ if (elen == sizeof(*elems->s1g_bcn_compat))
	1366	+ elems->s1g_bcn_compat = (void *)pos;
	1367	+ else
	1368	+ elem_parse_failed = true;
	1369	+ break;
	1370	+ case WLAN_EID_AID_RESPONSE:
	1371	+ if (elen == sizeof(struct ieee80211_aid_response_ie))
	1372	+ elems->aid_resp = (void *)pos;
	1373	+ else
	1374	+ elem_parse_failed = true;
1120	1375	break;
1121	1376	default:
1122	1377	break;
..	..	@@ -1126,13 +1381,143 @@
1126	1381	elems->parse_error = true;
1127	1382	else
1128	1383	__set_bit(id, seen_elems);
1129		-
1130		- left -= elen;
1131		- pos += elen;
1132	1384	}
1133	1385
1134		- if (left != 0)
	1386	+ if (!for_each_element_completed(elem, start, len))
1135	1387	elems->parse_error = true;
	1388	+
	1389	+ return crc;
	1390	+}
	1391	+
	1392	+static size_t ieee802_11_find_bssid_profile(const u8 *start, size_t len,
	1393	+ struct ieee802_11_elems *elems,
	1394	+ u8 *transmitter_bssid,
	1395	+ u8 *bss_bssid,
	1396	+ u8 *nontransmitted_profile)
	1397	+{
	1398	+ const struct element elem, sub;
	1399	+ size_t profile_len = 0;
	1400	+ bool found = false;
	1401	+
	1402	+ if (!bss_bssid \|\| !transmitter_bssid)
	1403	+ return profile_len;
	1404	+
	1405	+ for_each_element_id(elem, WLAN_EID_MULTIPLE_BSSID, start, len) {
	1406	+ if (elem->datalen < 2)
	1407	+ continue;
	1408	+ if (elem->data[0] < 1 \|\| elem->data[0] > 8)
	1409	+ continue;
	1410	+
	1411	+ for_each_element(sub, elem->data + 1, elem->datalen - 1) {
	1412	+ u8 new_bssid[ETH_ALEN];
	1413	+ const u8 *index;
	1414	+
	1415	+ if (sub->id != 0 \|\| sub->datalen < 4) {
	1416	+ /* not a valid BSS profile */
	1417	+ continue;
	1418	+ }
	1419	+
	1420	+ if (sub->data[0] != WLAN_EID_NON_TX_BSSID_CAP \|\|
	1421	+ sub->data[1] != 2) {
	1422	+ /* The first element of the
	1423	+ * Nontransmitted BSSID Profile is not
	1424	+ * the Nontransmitted BSSID Capability
	1425	+ * element.
	1426	+ */
	1427	+ continue;
	1428	+ }
	1429	+
	1430	+ memset(nontransmitted_profile, 0, len);
	1431	+ profile_len = cfg80211_merge_profile(start, len,
	1432	+ elem,
	1433	+ sub,
	1434	+ nontransmitted_profile,
	1435	+ len);
	1436	+
	1437	+ /* found a Nontransmitted BSSID Profile */
	1438	+ index = cfg80211_find_ie(WLAN_EID_MULTI_BSSID_IDX,
	1439	+ nontransmitted_profile,
	1440	+ profile_len);
	1441	+ if (!index \|\| index[1] < 1 \|\| index[2] == 0) {
	1442	+ /* Invalid MBSSID Index element */
	1443	+ continue;
	1444	+ }
	1445	+
	1446	+ cfg80211_gen_new_bssid(transmitter_bssid,
	1447	+ elem->data[0],
	1448	+ index[2],
	1449	+ new_bssid);
	1450	+ if (ether_addr_equal(new_bssid, bss_bssid)) {
	1451	+ found = true;
	1452	+ elems->bssid_index_len = index[1];
	1453	+ elems->bssid_index = (void *)&index[2];
	1454	+ break;
	1455	+ }
	1456	+ }
	1457	+ }
	1458	+
	1459	+ return found ? profile_len : 0;
	1460	+}
	1461	+
	1462	+u32 ieee802_11_parse_elems_crc(const u8 *start, size_t len, bool action,
	1463	+ struct ieee802_11_elems *elems,
	1464	+ u64 filter, u32 crc, u8 *transmitter_bssid,
	1465	+ u8 *bss_bssid)
	1466	+{
	1467	+ const struct element *non_inherit = NULL;
	1468	+ u8 *nontransmitted_profile;
	1469	+ int nontransmitted_profile_len = 0;
	1470	+
	1471	+ memset(elems, 0, sizeof(*elems));
	1472	+ elems->ie_start = start;
	1473	+ elems->total_len = len;
	1474	+
	1475	+ nontransmitted_profile = kmalloc(len, GFP_ATOMIC);
	1476	+ if (nontransmitted_profile) {
	1477	+ nontransmitted_profile_len =
	1478	+ ieee802_11_find_bssid_profile(start, len, elems,
	1479	+ transmitter_bssid,
	1480	+ bss_bssid,
	1481	+ nontransmitted_profile);
	1482	+ non_inherit =
	1483	+ cfg80211_find_ext_elem(WLAN_EID_EXT_NON_INHERITANCE,
	1484	+ nontransmitted_profile,
	1485	+ nontransmitted_profile_len);
	1486	+ if (!nontransmitted_profile_len) {
	1487	+ nontransmitted_profile_len = 0;
	1488	+ kfree(nontransmitted_profile);
	1489	+ nontransmitted_profile = NULL;
	1490	+ }
	1491	+ }
	1492	+
	1493	+ crc = _ieee802_11_parse_elems_crc(start, len, action, elems, filter,
	1494	+ crc, non_inherit);
	1495	+
	1496	+ /* Override with nontransmitted profile, if found */
	1497	+ if (nontransmitted_profile_len)
	1498	+ _ieee802_11_parse_elems_crc(nontransmitted_profile,
	1499	+ nontransmitted_profile_len,
	1500	+ action, elems, 0, 0, NULL);
	1501	+
	1502	+ if (elems->tim && !elems->parse_error) {
	1503	+ const struct ieee80211_tim_ie *tim_ie = elems->tim;
	1504	+
	1505	+ elems->dtim_period = tim_ie->dtim_period;
	1506	+ elems->dtim_count = tim_ie->dtim_count;
	1507	+ }
	1508	+
	1509	+ /* Override DTIM period and count if needed */
	1510	+ if (elems->bssid_index &&
	1511	+ elems->bssid_index_len >=
	1512	+ offsetofend(struct ieee80211_bssid_index, dtim_period))
	1513	+ elems->dtim_period = elems->bssid_index->dtim_period;
	1514	+
	1515	+ if (elems->bssid_index &&
	1516	+ elems->bssid_index_len >=
	1517	+ offsetofend(struct ieee80211_bssid_index, dtim_count))
	1518	+ elems->dtim_count = elems->bssid_index->dtim_count;
	1519	+
	1520	+ elems->nontx_profile = nontransmitted_profile;
1136	1521
1137	1522	return crc;
1138	1523	}
..	..	@@ -1332,7 +1717,8 @@
1332	1717	}
1333	1718
1334	1719	void ieee80211_send_deauth_disassoc(struct ieee80211_sub_if_data *sdata,
1335		- const u8 *bssid, u16 stype, u16 reason,
	1720	+ const u8 da, const u8 bssid,
	1721	+ u16 stype, u16 reason,
1336	1722	bool send_frame, u8 *frame_buf)
1337	1723	{
1338	1724	struct ieee80211_local *local = sdata->local;
..	..	@@ -1343,7 +1729,7 @@
1343	1729	mgmt->frame_control = cpu_to_le16(IEEE80211_FTYPE_MGMT \| stype);
1344	1730	mgmt->duration = 0; /* initialize only */
1345	1731	mgmt->seq_ctrl = 0; /* initialize only */
1346		- memcpy(mgmt->da, bssid, ETH_ALEN);
	1732	+ memcpy(mgmt->da, da, ETH_ALEN);
1347	1733	memcpy(mgmt->sa, sdata->vif.addr, ETH_ALEN);
1348	1734	memcpy(mgmt->bssid, bssid, ETH_ALEN);
1349	1735	/* u.deauth.reason_code == u.disassoc.reason_code */
..	..	@@ -1369,7 +1755,20 @@
1369	1755	}
1370	1756	}
1371	1757
1372		-static int ieee80211_build_preq_ies_band(struct ieee80211_local *local,
	1758	+static u8 ieee80211_write_he_6ghz_cap(u8 pos, __le16 cap, u8 *end)
	1759	+{
	1760	+ if ((end - pos) < 5)
	1761	+ return pos;
	1762	+
	1763	+ *pos++ = WLAN_EID_EXTENSION;
	1764	+ *pos++ = 1 + sizeof(cap);
	1765	+ *pos++ = WLAN_EID_EXT_HE_6GHZ_CAPA;
	1766	+ memcpy(pos, &cap, sizeof(cap));
	1767	+
	1768	+ return pos + 2;
	1769	+}
	1770	+
	1771	+static int ieee80211_build_preq_ies_band(struct ieee80211_sub_if_data *sdata,
1373	1772	u8 *buffer, size_t buffer_len,
1374	1773	const u8 *ie, size_t ie_len,
1375	1774	enum nl80211_band band,
..	..	@@ -1377,6 +1776,7 @@
1377	1776	struct cfg80211_chan_def *chandef,
1378	1777	size_t *offset, u32 flags)
1379	1778	{
	1779	+ struct ieee80211_local *local = sdata->local;
1380	1780	struct ieee80211_supported_band *sband;
1381	1781	const struct ieee80211_sta_he_cap *he_cap;
1382	1782	u8 pos = buffer, end = buffer + buffer_len;
..	..	@@ -1554,6 +1954,14 @@
1554	1954	pos = ieee80211_ie_build_he_cap(pos, he_cap, end);
1555	1955	if (!pos)
1556	1956	goto out_err;
	1957	+
	1958	+ if (sband->band == NL80211_BAND_6GHZ) {
	1959	+ enum nl80211_iftype iftype =
	1960	+ ieee80211_vif_type_p2p(&sdata->vif);
	1961	+ __le16 cap = ieee80211_get_he_6ghz_capa(sband, iftype);
	1962	+
	1963	+ pos = ieee80211_write_he_6ghz_cap(pos, cap, end);
	1964	+ }
1557	1965	}
1558	1966
1559	1967	/*
..	..	@@ -1568,7 +1976,7 @@
1568	1976	return pos - buffer;
1569	1977	}
1570	1978
1571		-int ieee80211_build_preq_ies(struct ieee80211_local local, u8 buffer,
	1979	+int ieee80211_build_preq_ies(struct ieee80211_sub_if_data sdata, u8 buffer,
1572	1980	size_t buffer_len,
1573	1981	struct ieee80211_scan_ies *ie_desc,
1574	1982	const u8 *ie, size_t ie_len,
..	..	@@ -1583,7 +1991,7 @@
1583	1991
1584	1992	for (i = 0; i < NUM_NL80211_BANDS; i++) {
1585	1993	if (bands_used & BIT(i)) {
1586		- pos += ieee80211_build_preq_ies_band(local,
	1994	+ pos += ieee80211_build_preq_ies_band(sdata,
1587	1995	buffer + pos,
1588	1996	buffer_len - pos,
1589	1997	ie, ie_len, i,
..	..	@@ -1645,7 +2053,7 @@
1645	2053	return NULL;
1646	2054
1647	2055	rate_masks[chan->band] = ratemask;
1648		- ies_len = ieee80211_build_preq_ies(local, skb_tail_pointer(skb),
	2056	+ ies_len = ieee80211_build_preq_ies(sdata, skb_tail_pointer(skb),
1649	2057	skb_tailroom(skb), &dummy_ie_desc,
1650	2058	ie, ie_len, BIT(chan->band),
1651	2059	rate_masks, &chandef, flags);
..	..	@@ -2031,10 +2439,10 @@
2031	2439	case NL80211_IFTYPE_ADHOC:
2032	2440	if (sdata->vif.bss_conf.ibss_joined)
2033	2441	WARN_ON(drv_join_ibss(local, sdata));
2034		- /* fall through */
	2442	+ fallthrough;
2035	2443	default:
2036	2444	ieee80211_reconfig_stations(sdata);
2037		- /* fall through */
	2445	+ fallthrough;
2038	2446	case NL80211_IFTYPE_AP: /* AP stations are handled later */
2039	2447	for (i = 0; i < IEEE80211_NUM_ACS; i++)
2040	2448	drv_conf_tx(local, sdata, i,
..	..	@@ -2083,9 +2491,14 @@
2083	2491	break;
2084	2492	case NL80211_IFTYPE_ADHOC:
2085	2493	changed \|= BSS_CHANGED_IBSS;
2086		- /* fall through */
	2494	+ fallthrough;
2087	2495	case NL80211_IFTYPE_AP:
2088	2496	changed \|= BSS_CHANGED_SSID \| BSS_CHANGED_P2P_PS;
	2497	+
	2498	+ if (sdata->vif.bss_conf.ftm_responder == 1 &&
	2499	+ wiphy_ext_feature_isset(sdata->local->hw.wiphy,
	2500	+ NL80211_EXT_FEATURE_ENABLE_FTM_RESPONDER))
	2501	+ changed \|= BSS_CHANGED_FTM_RESPONDER;
2089	2502
2090	2503	if (sdata->vif.type == NL80211_IFTYPE_AP) {
2091	2504	changed \|= BSS_CHANGED_AP_PROBE_RESP;
..	..	@@ -2093,8 +2506,7 @@
2093	2506	if (rcu_access_pointer(sdata->u.ap.beacon))
2094	2507	drv_start_ap(local, sdata);
2095	2508	}
2096		-
2097		- /* fall through */
	2509	+ fallthrough;
2098	2510	case NL80211_IFTYPE_MESH_POINT:
2099	2511	if (sdata->vif.bss_conf.enable_beacon) {
2100	2512	changed \|= BSS_CHANGED_BEACON \|
..	..	@@ -2164,11 +2576,7 @@
2164	2576
2165	2577	/* add back keys */
2166	2578	list_for_each_entry(sdata, &local->interfaces, list)
2167		- ieee80211_reset_crypto_tx_tailroom(sdata);
2168		-
2169		- list_for_each_entry(sdata, &local->interfaces, list)
2170		- if (ieee80211_sdata_running(sdata))
2171		- ieee80211_enable_keys(sdata);
	2579	+ ieee80211_reenable_keys(sdata);
2172	2580
2173	2581	/* Reconfigure sched scan if it was interrupted by FW restart */
2174	2582	mutex_lock(&local->mtx);
..	..	@@ -2457,6 +2865,27 @@
2457	2865	return pos;
2458	2866	}
2459	2867
	2868	+u8 ieee80211_ie_len_he_cap(struct ieee80211_sub_if_data *sdata, u8 iftype)
	2869	+{
	2870	+ const struct ieee80211_sta_he_cap *he_cap;
	2871	+ struct ieee80211_supported_band *sband;
	2872	+ u8 n;
	2873	+
	2874	+ sband = ieee80211_get_sband(sdata);
	2875	+ if (!sband)
	2876	+ return 0;
	2877	+
	2878	+ he_cap = ieee80211_get_he_iftype_cap(sband, iftype);
	2879	+ if (!he_cap)
	2880	+ return 0;
	2881	+
	2882	+ n = ieee80211_he_mcs_nss_size(&he_cap->he_cap_elem);
	2883	+ return 2 + 1 +
	2884	+ sizeof(he_cap->he_cap_elem) + n +
	2885	+ ieee80211_he_ppe_size(he_cap->ppe_thres[0],
	2886	+ he_cap->he_cap_elem.phy_cap_info);
	2887	+}
	2888	+
2460	2889	u8 ieee80211_ie_build_he_cap(u8 pos,
2461	2890	const struct ieee80211_sta_he_cap *he_cap,
2462	2891	u8 *end)
..	..	@@ -2521,6 +2950,54 @@
2521	2950	end:
2522	2951	orig_pos[1] = (pos - orig_pos) - 2;
2523	2952	return pos;
	2953	+}
	2954	+
	2955	+void ieee80211_ie_build_he_6ghz_cap(struct ieee80211_sub_if_data *sdata,
	2956	+ struct sk_buff *skb)
	2957	+{
	2958	+ struct ieee80211_supported_band *sband;
	2959	+ const struct ieee80211_sband_iftype_data *iftd;
	2960	+ enum nl80211_iftype iftype = ieee80211_vif_type_p2p(&sdata->vif);
	2961	+ u8 *pos;
	2962	+ u16 cap;
	2963	+
	2964	+ sband = ieee80211_get_sband(sdata);
	2965	+ if (!sband)
	2966	+ return;
	2967	+
	2968	+ iftd = ieee80211_get_sband_iftype_data(sband, iftype);
	2969	+ if (WARN_ON(!iftd))
	2970	+ return;
	2971	+
	2972	+ /* Check for device HE 6 GHz capability before adding element */
	2973	+ if (!iftd->he_6ghz_capa.capa)
	2974	+ return;
	2975	+
	2976	+ cap = le16_to_cpu(iftd->he_6ghz_capa.capa);
	2977	+ cap &= ~IEEE80211_HE_6GHZ_CAP_SM_PS;
	2978	+
	2979	+ switch (sdata->smps_mode) {
	2980	+ case IEEE80211_SMPS_AUTOMATIC:
	2981	+ case IEEE80211_SMPS_NUM_MODES:
	2982	+ WARN_ON(1);
	2983	+ fallthrough;
	2984	+ case IEEE80211_SMPS_OFF:
	2985	+ cap \|= u16_encode_bits(WLAN_HT_CAP_SM_PS_DISABLED,
	2986	+ IEEE80211_HE_6GHZ_CAP_SM_PS);
	2987	+ break;
	2988	+ case IEEE80211_SMPS_STATIC:
	2989	+ cap \|= u16_encode_bits(WLAN_HT_CAP_SM_PS_STATIC,
	2990	+ IEEE80211_HE_6GHZ_CAP_SM_PS);
	2991	+ break;
	2992	+ case IEEE80211_SMPS_DYNAMIC:
	2993	+ cap \|= u16_encode_bits(WLAN_HT_CAP_SM_PS_DYNAMIC,
	2994	+ IEEE80211_HE_6GHZ_CAP_SM_PS);
	2995	+ break;
	2996	+ }
	2997	+
	2998	+ pos = skb_put(skb, 2 + 1 + sizeof(cap));
	2999	+ ieee80211_write_he_6ghz_cap(pos, cpu_to_le16(cap),
	3000	+ pos + 2 + 1 + sizeof(cap));
2524	3001	}
2525	3002
2526	3003	u8 ieee80211_ie_build_ht_oper(u8 pos, struct ieee80211_sta_ht_cap *ht_cap,
..	..	@@ -2646,6 +3123,91 @@
2646	3123	return pos + sizeof(struct ieee80211_vht_operation);
2647	3124	}
2648	3125
	3126	+u8 ieee80211_ie_build_he_oper(u8 pos, struct cfg80211_chan_def *chandef)
	3127	+{
	3128	+ struct ieee80211_he_operation *he_oper;
	3129	+ struct ieee80211_he_6ghz_oper *he_6ghz_op;
	3130	+ u32 he_oper_params;
	3131	+ u8 ie_len = 1 + sizeof(struct ieee80211_he_operation);
	3132	+
	3133	+ if (chandef->chan->band == NL80211_BAND_6GHZ)
	3134	+ ie_len += sizeof(struct ieee80211_he_6ghz_oper);
	3135	+
	3136	+ *pos++ = WLAN_EID_EXTENSION;
	3137	+ *pos++ = ie_len;
	3138	+ *pos++ = WLAN_EID_EXT_HE_OPERATION;
	3139	+
	3140	+ he_oper_params = 0;
	3141	+ he_oper_params \|= u32_encode_bits(1023, /* disabled */
	3142	+ IEEE80211_HE_OPERATION_RTS_THRESHOLD_MASK);
	3143	+ he_oper_params \|= u32_encode_bits(1,
	3144	+ IEEE80211_HE_OPERATION_ER_SU_DISABLE);
	3145	+ he_oper_params \|= u32_encode_bits(1,
	3146	+ IEEE80211_HE_OPERATION_BSS_COLOR_DISABLED);
	3147	+ if (chandef->chan->band == NL80211_BAND_6GHZ)
	3148	+ he_oper_params \|= u32_encode_bits(1,
	3149	+ IEEE80211_HE_OPERATION_6GHZ_OP_INFO);
	3150	+
	3151	+ he_oper = (struct ieee80211_he_operation *)pos;
	3152	+ he_oper->he_oper_params = cpu_to_le32(he_oper_params);
	3153	+
	3154	+ /* don't require special HE peer rates */
	3155	+ he_oper->he_mcs_nss_set = cpu_to_le16(0xffff);
	3156	+ pos += sizeof(struct ieee80211_he_operation);
	3157	+
	3158	+ if (chandef->chan->band != NL80211_BAND_6GHZ)
	3159	+ goto out;
	3160	+
	3161	+ /* TODO add VHT operational */
	3162	+ he_6ghz_op = (struct ieee80211_he_6ghz_oper *)pos;
	3163	+ he_6ghz_op->minrate = 6; /* 6 Mbps */
	3164	+ he_6ghz_op->primary =
	3165	+ ieee80211_frequency_to_channel(chandef->chan->center_freq);
	3166	+ he_6ghz_op->ccfs0 =
	3167	+ ieee80211_frequency_to_channel(chandef->center_freq1);
	3168	+ if (chandef->center_freq2)
	3169	+ he_6ghz_op->ccfs1 =
	3170	+ ieee80211_frequency_to_channel(chandef->center_freq2);
	3171	+ else
	3172	+ he_6ghz_op->ccfs1 = 0;
	3173	+
	3174	+ switch (chandef->width) {
	3175	+ case NL80211_CHAN_WIDTH_160:
	3176	+ /* Convert 160 MHz channel width to new style as interop
	3177	+ * workaround.
	3178	+ */
	3179	+ he_6ghz_op->control =
	3180	+ IEEE80211_HE_6GHZ_OPER_CTRL_CHANWIDTH_160MHZ;
	3181	+ he_6ghz_op->ccfs1 = he_6ghz_op->ccfs0;
	3182	+ if (chandef->chan->center_freq < chandef->center_freq1)
	3183	+ he_6ghz_op->ccfs0 -= 8;
	3184	+ else
	3185	+ he_6ghz_op->ccfs0 += 8;
	3186	+ fallthrough;
	3187	+ case NL80211_CHAN_WIDTH_80P80:
	3188	+ he_6ghz_op->control =
	3189	+ IEEE80211_HE_6GHZ_OPER_CTRL_CHANWIDTH_160MHZ;
	3190	+ break;
	3191	+ case NL80211_CHAN_WIDTH_80:
	3192	+ he_6ghz_op->control =
	3193	+ IEEE80211_HE_6GHZ_OPER_CTRL_CHANWIDTH_80MHZ;
	3194	+ break;
	3195	+ case NL80211_CHAN_WIDTH_40:
	3196	+ he_6ghz_op->control =
	3197	+ IEEE80211_HE_6GHZ_OPER_CTRL_CHANWIDTH_40MHZ;
	3198	+ break;
	3199	+ default:
	3200	+ he_6ghz_op->control =
	3201	+ IEEE80211_HE_6GHZ_OPER_CTRL_CHANWIDTH_20MHZ;
	3202	+ break;
	3203	+ }
	3204	+
	3205	+ pos += sizeof(struct ieee80211_he_6ghz_oper);
	3206	+
	3207	+out:
	3208	+ return pos;
	3209	+}
	3210	+
2649	3211	bool ieee80211_chandef_ht_oper(const struct ieee80211_ht_operation *ht_oper,
2650	3212	struct cfg80211_chan_def *chandef)
2651	3213	{
..	..	@@ -2673,49 +3235,118 @@
2673	3235	return true;
2674	3236	}
2675	3237
2676		-bool ieee80211_chandef_vht_oper(const struct ieee80211_vht_operation *oper,
	3238	+bool ieee80211_chandef_vht_oper(struct ieee80211_hw *hw, u32 vht_cap_info,
	3239	+ const struct ieee80211_vht_operation *oper,
	3240	+ const struct ieee80211_ht_operation *htop,
2677	3241	struct cfg80211_chan_def *chandef)
2678	3242	{
2679	3243	struct cfg80211_chan_def new = *chandef;
2680		- int cf1, cf2;
	3244	+ int cf0, cf1;
	3245	+ int ccfs0, ccfs1, ccfs2;
	3246	+ int ccf0, ccf1;
	3247	+ u32 vht_cap;
	3248	+ bool support_80_80 = false;
	3249	+ bool support_160 = false;
	3250	+ u8 ext_nss_bw_supp = u32_get_bits(vht_cap_info,
	3251	+ IEEE80211_VHT_CAP_EXT_NSS_BW_MASK);
	3252	+ u8 supp_chwidth = u32_get_bits(vht_cap_info,
	3253	+ IEEE80211_VHT_CAP_SUPP_CHAN_WIDTH_MASK);
2681	3254
2682		- if (!oper)
	3255	+ if (!oper \|\| !htop)
2683	3256	return false;
2684	3257
2685		- cf1 = ieee80211_channel_to_frequency(oper->center_freq_seg0_idx,
2686		- chandef->chan->band);
2687		- cf2 = ieee80211_channel_to_frequency(oper->center_freq_seg1_idx,
2688		- chandef->chan->band);
	3258	+ vht_cap = hw->wiphy->bands[chandef->chan->band]->vht_cap.cap;
	3259	+ support_160 = (vht_cap & (IEEE80211_VHT_CAP_SUPP_CHAN_WIDTH_MASK \|
	3260	+ IEEE80211_VHT_CAP_EXT_NSS_BW_MASK));
	3261	+ support_80_80 = ((vht_cap &
	3262	+ IEEE80211_VHT_CAP_SUPP_CHAN_WIDTH_160_80PLUS80MHZ) \|\|
	3263	+ (vht_cap & IEEE80211_VHT_CAP_SUPP_CHAN_WIDTH_160MHZ &&
	3264	+ vht_cap & IEEE80211_VHT_CAP_EXT_NSS_BW_MASK) \|\|
	3265	+ ((vht_cap & IEEE80211_VHT_CAP_EXT_NSS_BW_MASK) >>
	3266	+ IEEE80211_VHT_CAP_EXT_NSS_BW_SHIFT > 1));
	3267	+ ccfs0 = oper->center_freq_seg0_idx;
	3268	+ ccfs1 = oper->center_freq_seg1_idx;
	3269	+ ccfs2 = (le16_to_cpu(htop->operation_mode) &
	3270	+ IEEE80211_HT_OP_MODE_CCFS2_MASK)
	3271	+ >> IEEE80211_HT_OP_MODE_CCFS2_SHIFT;
	3272	+
	3273	+ ccf0 = ccfs0;
	3274	+
	3275	+ /* if not supported, parse as though we didn't understand it */
	3276	+ if (!ieee80211_hw_check(hw, SUPPORTS_VHT_EXT_NSS_BW))
	3277	+ ext_nss_bw_supp = 0;
	3278	+
	3279	+ /*
	3280	+ * Cf. IEEE 802.11 Table 9-250
	3281	+ *
	3282	+ * We really just consider that because it's inefficient to connect
	3283	+ * at a higher bandwidth than we'll actually be able to use.
	3284	+ */
	3285	+ switch ((supp_chwidth << 4) \| ext_nss_bw_supp) {
	3286	+ default:
	3287	+ case 0x00:
	3288	+ ccf1 = 0;
	3289	+ support_160 = false;
	3290	+ support_80_80 = false;
	3291	+ break;
	3292	+ case 0x01:
	3293	+ support_80_80 = false;
	3294	+ fallthrough;
	3295	+ case 0x02:
	3296	+ case 0x03:
	3297	+ ccf1 = ccfs2;
	3298	+ break;
	3299	+ case 0x10:
	3300	+ ccf1 = ccfs1;
	3301	+ break;
	3302	+ case 0x11:
	3303	+ case 0x12:
	3304	+ if (!ccfs1)
	3305	+ ccf1 = ccfs2;
	3306	+ else
	3307	+ ccf1 = ccfs1;
	3308	+ break;
	3309	+ case 0x13:
	3310	+ case 0x20:
	3311	+ case 0x23:
	3312	+ ccf1 = ccfs1;
	3313	+ break;
	3314	+ }
	3315	+
	3316	+ cf0 = ieee80211_channel_to_frequency(ccf0, chandef->chan->band);
	3317	+ cf1 = ieee80211_channel_to_frequency(ccf1, chandef->chan->band);
2689	3318
2690	3319	switch (oper->chan_width) {
2691	3320	case IEEE80211_VHT_CHANWIDTH_USE_HT:
	3321	+ /* just use HT information directly */
2692	3322	break;
2693	3323	case IEEE80211_VHT_CHANWIDTH_80MHZ:
2694	3324	new.width = NL80211_CHAN_WIDTH_80;
2695		- new.center_freq1 = cf1;
	3325	+ new.center_freq1 = cf0;
2696	3326	/* If needed, adjust based on the newer interop workaround. */
2697		- if (oper->center_freq_seg1_idx) {
	3327	+ if (ccf1) {
2698	3328	unsigned int diff;
2699	3329
2700		- diff = abs(oper->center_freq_seg1_idx -
2701		- oper->center_freq_seg0_idx);
2702		- if (diff == 8) {
	3330	+ diff = abs(ccf1 - ccf0);
	3331	+ if ((diff == 8) && support_160) {
2703	3332	new.width = NL80211_CHAN_WIDTH_160;
2704		- new.center_freq1 = cf2;
2705		- } else if (diff > 8) {
	3333	+ new.center_freq1 = cf1;
	3334	+ } else if ((diff > 8) && support_80_80) {
2706	3335	new.width = NL80211_CHAN_WIDTH_80P80;
2707		- new.center_freq2 = cf2;
	3336	+ new.center_freq2 = cf1;
2708	3337	}
2709	3338	}
2710	3339	break;
2711	3340	case IEEE80211_VHT_CHANWIDTH_160MHZ:
	3341	+ /* deprecated encoding */
2712	3342	new.width = NL80211_CHAN_WIDTH_160;
2713		- new.center_freq1 = cf1;
	3343	+ new.center_freq1 = cf0;
2714	3344	break;
2715	3345	case IEEE80211_VHT_CHANWIDTH_80P80MHZ:
	3346	+ /* deprecated encoding */
2716	3347	new.width = NL80211_CHAN_WIDTH_80P80;
2717		- new.center_freq1 = cf1;
2718		- new.center_freq2 = cf2;
	3348	+ new.center_freq1 = cf0;
	3349	+ new.center_freq2 = cf1;
2719	3350	break;
2720	3351	default:
2721	3352	return false;
..	..	@@ -2725,6 +3356,149 @@
2725	3356	return false;
2726	3357
2727	3358	*chandef = new;
	3359	+ return true;
	3360	+}
	3361	+
	3362	+bool ieee80211_chandef_he_6ghz_oper(struct ieee80211_sub_if_data *sdata,
	3363	+ const struct ieee80211_he_operation *he_oper,
	3364	+ struct cfg80211_chan_def *chandef)
	3365	+{
	3366	+ struct ieee80211_local *local = sdata->local;
	3367	+ struct ieee80211_supported_band *sband;
	3368	+ enum nl80211_iftype iftype = ieee80211_vif_type_p2p(&sdata->vif);
	3369	+ const struct ieee80211_sta_he_cap *he_cap;
	3370	+ struct cfg80211_chan_def he_chandef = *chandef;
	3371	+ const struct ieee80211_he_6ghz_oper *he_6ghz_oper;
	3372	+ bool support_80_80, support_160;
	3373	+ u8 he_phy_cap;
	3374	+ u32 freq;
	3375	+
	3376	+ if (chandef->chan->band != NL80211_BAND_6GHZ)
	3377	+ return true;
	3378	+
	3379	+ sband = local->hw.wiphy->bands[NL80211_BAND_6GHZ];
	3380	+
	3381	+ he_cap = ieee80211_get_he_iftype_cap(sband, iftype);
	3382	+ if (!he_cap) {
	3383	+ sdata_info(sdata, "Missing iftype sband data/HE cap");
	3384	+ return false;
	3385	+ }
	3386	+
	3387	+ he_phy_cap = he_cap->he_cap_elem.phy_cap_info[0];
	3388	+ support_160 =
	3389	+ he_phy_cap &
	3390	+ IEEE80211_HE_PHY_CAP0_CHANNEL_WIDTH_SET_160MHZ_IN_5G;
	3391	+ support_80_80 =
	3392	+ he_phy_cap &
	3393	+ IEEE80211_HE_PHY_CAP0_CHANNEL_WIDTH_SET_80PLUS80_MHZ_IN_5G;
	3394	+
	3395	+ if (!he_oper) {
	3396	+ sdata_info(sdata,
	3397	+ "HE is not advertised on (on %d MHz), expect issues\n",
	3398	+ chandef->chan->center_freq);
	3399	+ return false;
	3400	+ }
	3401	+
	3402	+ he_6ghz_oper = ieee80211_he_6ghz_oper(he_oper);
	3403	+
	3404	+ if (!he_6ghz_oper) {
	3405	+ sdata_info(sdata,
	3406	+ "HE 6GHz operation missing (on %d MHz), expect issues\n",
	3407	+ chandef->chan->center_freq);
	3408	+ return false;
	3409	+ }
	3410	+
	3411	+ freq = ieee80211_channel_to_frequency(he_6ghz_oper->primary,
	3412	+ NL80211_BAND_6GHZ);
	3413	+ he_chandef.chan = ieee80211_get_channel(sdata->local->hw.wiphy, freq);
	3414	+
	3415	+ switch (u8_get_bits(he_6ghz_oper->control,
	3416	+ IEEE80211_HE_6GHZ_OPER_CTRL_CHANWIDTH)) {
	3417	+ case IEEE80211_HE_6GHZ_OPER_CTRL_CHANWIDTH_20MHZ:
	3418	+ he_chandef.width = NL80211_CHAN_WIDTH_20;
	3419	+ break;
	3420	+ case IEEE80211_HE_6GHZ_OPER_CTRL_CHANWIDTH_40MHZ:
	3421	+ he_chandef.width = NL80211_CHAN_WIDTH_40;
	3422	+ break;
	3423	+ case IEEE80211_HE_6GHZ_OPER_CTRL_CHANWIDTH_80MHZ:
	3424	+ he_chandef.width = NL80211_CHAN_WIDTH_80;
	3425	+ break;
	3426	+ case IEEE80211_HE_6GHZ_OPER_CTRL_CHANWIDTH_160MHZ:
	3427	+ he_chandef.width = NL80211_CHAN_WIDTH_80;
	3428	+ if (!he_6ghz_oper->ccfs1)
	3429	+ break;
	3430	+ if (abs(he_6ghz_oper->ccfs1 - he_6ghz_oper->ccfs0) == 8) {
	3431	+ if (support_160)
	3432	+ he_chandef.width = NL80211_CHAN_WIDTH_160;
	3433	+ } else {
	3434	+ if (support_80_80)
	3435	+ he_chandef.width = NL80211_CHAN_WIDTH_80P80;
	3436	+ }
	3437	+ break;
	3438	+ }
	3439	+
	3440	+ if (he_chandef.width == NL80211_CHAN_WIDTH_160) {
	3441	+ he_chandef.center_freq1 =
	3442	+ ieee80211_channel_to_frequency(he_6ghz_oper->ccfs1,
	3443	+ NL80211_BAND_6GHZ);
	3444	+ } else {
	3445	+ he_chandef.center_freq1 =
	3446	+ ieee80211_channel_to_frequency(he_6ghz_oper->ccfs0,
	3447	+ NL80211_BAND_6GHZ);
	3448	+ if (support_80_80 \|\| support_160)
	3449	+ he_chandef.center_freq2 =
	3450	+ ieee80211_channel_to_frequency(he_6ghz_oper->ccfs1,
	3451	+ NL80211_BAND_6GHZ);
	3452	+ }
	3453	+
	3454	+ if (!cfg80211_chandef_valid(&he_chandef)) {
	3455	+ sdata_info(sdata,
	3456	+ "HE 6GHz operation resulted in invalid chandef: %d MHz/%d/%d MHz/%d MHz\n",
	3457	+ he_chandef.chan ? he_chandef.chan->center_freq : 0,
	3458	+ he_chandef.width,
	3459	+ he_chandef.center_freq1,
	3460	+ he_chandef.center_freq2);
	3461	+ return false;
	3462	+ }
	3463	+
	3464	+ *chandef = he_chandef;
	3465	+
	3466	+ return true;
	3467	+}
	3468	+
	3469	+bool ieee80211_chandef_s1g_oper(const struct ieee80211_s1g_oper_ie *oper,
	3470	+ struct cfg80211_chan_def *chandef)
	3471	+{
	3472	+ u32 oper_freq;
	3473	+
	3474	+ if (!oper)
	3475	+ return false;
	3476	+
	3477	+ switch (FIELD_GET(S1G_OPER_CH_WIDTH_OPER, oper->ch_width)) {
	3478	+ case IEEE80211_S1G_CHANWIDTH_1MHZ:
	3479	+ chandef->width = NL80211_CHAN_WIDTH_1;
	3480	+ break;
	3481	+ case IEEE80211_S1G_CHANWIDTH_2MHZ:
	3482	+ chandef->width = NL80211_CHAN_WIDTH_2;
	3483	+ break;
	3484	+ case IEEE80211_S1G_CHANWIDTH_4MHZ:
	3485	+ chandef->width = NL80211_CHAN_WIDTH_4;
	3486	+ break;
	3487	+ case IEEE80211_S1G_CHANWIDTH_8MHZ:
	3488	+ chandef->width = NL80211_CHAN_WIDTH_8;
	3489	+ break;
	3490	+ case IEEE80211_S1G_CHANWIDTH_16MHZ:
	3491	+ chandef->width = NL80211_CHAN_WIDTH_16;
	3492	+ break;
	3493	+ default:
	3494	+ return false;
	3495	+ }
	3496	+
	3497	+ oper_freq = ieee80211_channel_to_freq_khz(oper->oper_ch,
	3498	+ NL80211_BAND_S1GHZ);
	3499	+ chandef->center_freq1 = KHZ_TO_MHZ(oper_freq);
	3500	+ chandef->freq1_offset = oper_freq % 1000;
	3501	+
2728	3502	return true;
2729	3503	}
2730	3504
..	..	@@ -2924,7 +3698,7 @@
2924	3698	break;
2925	3699	default:
2926	3700	WARN_ON(1);
2927		- /* fall through */
	3701	+ fallthrough;
2928	3702	case RX_ENC_LEGACY: {
2929	3703	struct ieee80211_supported_band *sband;
2930	3704	int shift = 0;
..	..	@@ -3088,6 +3862,11 @@
3088	3862	c->width = NL80211_CHAN_WIDTH_20_NOHT;
3089	3863	ret = IEEE80211_STA_DISABLE_HT \| IEEE80211_STA_DISABLE_VHT;
3090	3864	break;
	3865	+ case NL80211_CHAN_WIDTH_1:
	3866	+ case NL80211_CHAN_WIDTH_2:
	3867	+ case NL80211_CHAN_WIDTH_4:
	3868	+ case NL80211_CHAN_WIDTH_8:
	3869	+ case NL80211_CHAN_WIDTH_16:
3091	3870	case NL80211_CHAN_WIDTH_5:
3092	3871	case NL80211_CHAN_WIDTH_10:
3093	3872	WARN_ON_ONCE(1);
..	..	@@ -3626,6 +4405,58 @@
3626	4405	return max_num_different_channels;
3627	4406	}
3628	4407
	4408	+void ieee80211_add_s1g_capab_ie(struct ieee80211_sub_if_data *sdata,
	4409	+ struct ieee80211_sta_s1g_cap *caps,
	4410	+ struct sk_buff *skb)
	4411	+{
	4412	+ struct ieee80211_if_managed *ifmgd = &sdata->u.mgd;
	4413	+ struct ieee80211_s1g_cap s1g_capab;
	4414	+ u8 *pos;
	4415	+ int i;
	4416	+
	4417	+ if (WARN_ON(sdata->vif.type != NL80211_IFTYPE_STATION))
	4418	+ return;
	4419	+
	4420	+ if (!caps->s1g)
	4421	+ return;
	4422	+
	4423	+ memcpy(s1g_capab.capab_info, caps->cap, sizeof(caps->cap));
	4424	+ memcpy(s1g_capab.supp_mcs_nss, caps->nss_mcs, sizeof(caps->nss_mcs));
	4425	+
	4426	+ /* override the capability info */
	4427	+ for (i = 0; i < sizeof(ifmgd->s1g_capa.capab_info); i++) {
	4428	+ u8 mask = ifmgd->s1g_capa_mask.capab_info[i];
	4429	+
	4430	+ s1g_capab.capab_info[i] &= ~mask;
	4431	+ s1g_capab.capab_info[i] \|= ifmgd->s1g_capa.capab_info[i] & mask;
	4432	+ }
	4433	+
	4434	+ /* then MCS and NSS set */
	4435	+ for (i = 0; i < sizeof(ifmgd->s1g_capa.supp_mcs_nss); i++) {
	4436	+ u8 mask = ifmgd->s1g_capa_mask.supp_mcs_nss[i];
	4437	+
	4438	+ s1g_capab.supp_mcs_nss[i] &= ~mask;
	4439	+ s1g_capab.supp_mcs_nss[i] \|=
	4440	+ ifmgd->s1g_capa.supp_mcs_nss[i] & mask;
	4441	+ }
	4442	+
	4443	+ pos = skb_put(skb, 2 + sizeof(s1g_capab));
	4444	+ *pos++ = WLAN_EID_S1G_CAPABILITIES;
	4445	+ *pos++ = sizeof(s1g_capab);
	4446	+
	4447	+ memcpy(pos, &s1g_capab, sizeof(s1g_capab));
	4448	+}
	4449	+
	4450	+void ieee80211_add_aid_request_ie(struct ieee80211_sub_if_data *sdata,
	4451	+ struct sk_buff *skb)
	4452	+{
	4453	+ u8 *pos = skb_put(skb, 3);
	4454	+
	4455	+ *pos++ = WLAN_EID_AID_REQUEST;
	4456	+ *pos++ = 1;
	4457	+ *pos++ = 0;
	4458	+}
	4459	+
3629	4460	u8 ieee80211_add_wmm_info_ie(u8 buf, u8 qosinfo)
3630	4461	{
3631	4462	*buf++ = WLAN_EID_VENDOR_SPECIFIC;
..	..	@@ -3668,3 +4499,24 @@
3668	4499	IEEE80211_WMM_IE_STA_QOSINFO_AC_BE,
3669	4500	IEEE80211_WMM_IE_STA_QOSINFO_AC_BK
3670	4501	};
	4502	+
	4503	+u16 ieee80211_encode_usf(int listen_interval)
	4504	+{
	4505	+ static const int listen_int_usf[] = { 1, 10, 1000, 10000 };
	4506	+ u16 ui, usf = 0;
	4507	+
	4508	+ /* find greatest USF */
	4509	+ while (usf < IEEE80211_MAX_USF) {
	4510	+ if (listen_interval % listen_int_usf[usf + 1])
	4511	+ break;
	4512	+ usf += 1;
	4513	+ }
	4514	+ ui = listen_interval / listen_int_usf[usf];
	4515	+
	4516	+ /* error if there is a remainder. Should've been checked by user */
	4517	+ WARN_ON_ONCE(ui > IEEE80211_MAX_UI);
	4518	+ listen_interval = FIELD_PREP(LISTEN_INT_USF, usf) \|
	4519	+ FIELD_PREP(LISTEN_INT_UI, ui);
	4520	+
	4521	+ return (u16) listen_interval;
	4522	+}