1	// SPDX-License-Identifier: GPL-2.0-only
2	/*
3	* mac80211 TDLS handling code
4	*
5	* Copyright 2006-2010 Johannes Berg <johannes@sipsolutions.net>
6	* Copyright 2014, Intel Corporation
7	* Copyright 2014 Intel Mobile Communications GmbH
8	* Copyright 2015 - 2016 Intel Deutschland GmbH
9	* Copyright (C) 2019, 2021-2023 Intel Corporation
10	*/
11
12	#include <linux/ieee80211.h>
13	#include <linux/log2.h>
14	#include <net/cfg80211.h>
15	#include <linux/rtnetlink.h>
16	#include "ieee80211_i.h"
17	#include "driver-ops.h"
18	#include "rate.h"
19	#include "wme.h"
20
21	/* give usermode some time for retries in setting up the TDLS session */
22	#define TDLS_PEER_SETUP_TIMEOUT (15 * HZ)
23
24	void ieee80211_tdls_peer_del_work(struct wiphy *wiphy, struct wiphy_work *wk)
25	{
26	struct ieee80211_sub_if_data *sdata;
27	struct ieee80211_local *local;
28
29	sdata = container_of(wk, struct ieee80211_sub_if_data,
30	u.mgd.tdls_peer_del_work.work);
31	local = sdata->local;
32
33	lockdep_assert_wiphy(local->hw.wiphy);
34
35	if (!is_zero_ether_addr(addr: sdata->u.mgd.tdls_peer)) {
36	tdls_dbg(sdata, "TDLS del peer %pM\n", sdata->u.mgd.tdls_peer);
37	sta_info_destroy_addr(sdata, addr: sdata->u.mgd.tdls_peer);
38	eth_zero_addr(addr: sdata->u.mgd.tdls_peer);
39	}
40	}
41
42	static void ieee80211_tdls_add_ext_capab(struct ieee80211_link_data *link,
43	struct sk_buff *skb)
44	{
45	struct ieee80211_sub_if_data *sdata = link->sdata;
46	struct ieee80211_local *local = sdata->local;
47	struct ieee80211_if_managed *ifmgd = &sdata->u.mgd;
48	bool chan_switch = local->hw.wiphy->features &
49	NL80211_FEATURE_TDLS_CHANNEL_SWITCH;
50	bool wider_band = ieee80211_hw_check(&local->hw, TDLS_WIDER_BW) &&
51	!ifmgd->tdls_wider_bw_prohibited;
52	bool buffer_sta = ieee80211_hw_check(&local->hw,
53	SUPPORTS_TDLS_BUFFER_STA);
54	struct ieee80211_supported_band *sband = ieee80211_get_link_sband(link);
55	bool vht = sband && sband->vht_cap.vht_supported;
56	u8 *pos = skb_put(skb, len: 10);
57
58	*pos++ = WLAN_EID_EXT_CAPABILITY;
59	*pos++ = 8; /* len */
60	*pos++ = 0x0;
61	*pos++ = 0x0;
62	*pos++ = 0x0;
63	*pos++ = (chan_switch ? WLAN_EXT_CAPA4_TDLS_CHAN_SWITCH : 0) |
64	(buffer_sta ? WLAN_EXT_CAPA4_TDLS_BUFFER_STA : 0);
65	*pos++ = WLAN_EXT_CAPA5_TDLS_ENABLED;
66	*pos++ = 0;
67	*pos++ = 0;
68	*pos++ = (vht && wider_band) ? WLAN_EXT_CAPA8_TDLS_WIDE_BW_ENABLED : 0;
69	}
70
71	static u8
72	ieee80211_tdls_add_subband(struct ieee80211_sub_if_data *sdata,
73	struct sk_buff *skb, u16 start, u16 end,
74	u16 spacing)
75	{
76	u8 subband_cnt = 0, ch_cnt = 0;
77	struct ieee80211_channel *ch;
78	struct cfg80211_chan_def chandef;
79	int i, subband_start;
80	struct wiphy *wiphy = sdata->local->hw.wiphy;
81
82	for (i = start; i <= end; i += spacing) {
83	if (!ch_cnt)
84	subband_start = i;
85
86	ch = ieee80211_get_channel(wiphy: sdata->local->hw.wiphy, freq: i);
87	if (ch) {
88	/* we will be active on the channel */
89	cfg80211_chandef_create(chandef: &chandef, channel: ch,
90	chantype: NL80211_CHAN_NO_HT);
91	if (cfg80211_reg_can_beacon_relax(wiphy, chandef: &chandef,
92	iftype: sdata->wdev.iftype)) {
93	ch_cnt++;
94	/*
95	* check if the next channel is also part of
96	* this allowed range
97	*/
98	continue;
99	}
100	}
101
102	/*
103	* we've reached the end of a range, with allowed channels
104	* found
105	*/
106	if (ch_cnt) {
107	u8 *pos = skb_put(skb, len: 2);
108	*pos++ = ieee80211_frequency_to_channel(freq: subband_start);
109	*pos++ = ch_cnt;
110
111	subband_cnt++;
112	ch_cnt = 0;
113	}
114	}
115
116	/* all channels in the requested range are allowed - add them here */
117	if (ch_cnt) {
118	u8 *pos = skb_put(skb, len: 2);
119	*pos++ = ieee80211_frequency_to_channel(freq: subband_start);
120	*pos++ = ch_cnt;
121
122	subband_cnt++;
123	}
124
125	return subband_cnt;
126	}
127
128	static void
129	ieee80211_tdls_add_supp_channels(struct ieee80211_sub_if_data *sdata,
130	struct sk_buff *skb)
131	{
132	/*
133	* Add possible channels for TDLS. These are channels that are allowed
134	* to be active.
135	*/
136	u8 subband_cnt;
137	u8 *pos = skb_put(skb, len: 2);
138
139	*pos++ = WLAN_EID_SUPPORTED_CHANNELS;
140
141	/*
142	* 5GHz and 2GHz channels numbers can overlap. Ignore this for now, as
143	* this doesn't happen in real world scenarios.
144	*/
145
146	/* 2GHz, with 5MHz spacing */
147	subband_cnt = ieee80211_tdls_add_subband(sdata, skb, start: 2412, end: 2472, spacing: 5);
148
149	/* 5GHz, with 20MHz spacing */
150	subband_cnt += ieee80211_tdls_add_subband(sdata, skb, start: 5000, end: 5825, spacing: 20);
151
152	/* length */
153	*pos = 2 * subband_cnt;
154	}
155
156	static void ieee80211_tdls_add_oper_classes(struct ieee80211_link_data *link,
157	struct sk_buff *skb)
158	{
159	u8 *pos;
160	u8 op_class;
161
162	if (!ieee80211_chandef_to_operating_class(chandef: &link->conf->chandef,
163	op_class: &op_class))
164	return;
165
166	pos = skb_put(skb, len: 4);
167	*pos++ = WLAN_EID_SUPPORTED_REGULATORY_CLASSES;
168	*pos++ = 2; /* len */
169
170	*pos++ = op_class;
171	*pos++ = op_class; /* give current operating class as alternate too */
172	}
173
174	static void ieee80211_tdls_add_bss_coex_ie(struct sk_buff *skb)
175	{
176	u8 *pos = skb_put(skb, len: 3);
177
178	*pos++ = WLAN_EID_BSS_COEX_2040;
179	*pos++ = 1; /* len */
180
181	*pos++ = WLAN_BSS_COEX_INFORMATION_REQUEST;
182	}
183
184	static u16 ieee80211_get_tdls_sta_capab(struct ieee80211_link_data *link,
185	u16 status_code)
186	{
187	struct ieee80211_supported_band *sband;
188
189	/* The capability will be 0 when sending a failure code */
190	if (status_code != 0)
191	return 0;
192
193	sband = ieee80211_get_link_sband(link);
194
195	if (sband && sband->band == NL80211_BAND_2GHZ) {
196	return WLAN_CAPABILITY_SHORT_SLOT_TIME |
197	WLAN_CAPABILITY_SHORT_PREAMBLE;
198	}
199
200	return 0;
201	}
202
203	static void ieee80211_tdls_add_link_ie(struct ieee80211_link_data *link,
204	struct sk_buff *skb, const u8 *peer,
205	bool initiator)
206	{
207	struct ieee80211_sub_if_data *sdata = link->sdata;
208	struct ieee80211_tdls_lnkie *lnkid;
209	const u8 *init_addr, *rsp_addr;
210
211	if (initiator) {
212	init_addr = sdata->vif.addr;
213	rsp_addr = peer;
214	} else {
215	init_addr = peer;
216	rsp_addr = sdata->vif.addr;
217	}
218
219	lnkid = skb_put(skb, len: sizeof(struct ieee80211_tdls_lnkie));
220
221	lnkid->ie_type = WLAN_EID_LINK_ID;
222	lnkid->ie_len = sizeof(struct ieee80211_tdls_lnkie) - 2;
223
224	memcpy(lnkid->bssid, link->u.mgd.bssid, ETH_ALEN);
225	memcpy(lnkid->init_sta, init_addr, ETH_ALEN);
226	memcpy(lnkid->resp_sta, rsp_addr, ETH_ALEN);
227	}
228
229	static void
230	ieee80211_tdls_add_aid(struct ieee80211_sub_if_data *sdata, struct sk_buff *skb)
231	{
232	u8 *pos = skb_put(skb, len: 4);
233
234	*pos++ = WLAN_EID_AID;
235	*pos++ = 2; /* len */
236	put_unaligned_le16(val: sdata->vif.cfg.aid, p: pos);
237	}
238
239	/* translate numbering in the WMM parameter IE to the mac80211 notation */
240	static enum ieee80211_ac_numbers ieee80211_ac_from_wmm(int ac)
241	{
242	switch (ac) {
243	default:
244	WARN_ON_ONCE(1);
245	fallthrough;
246	case 0:
247	return IEEE80211_AC_BE;
248	case 1:
249	return IEEE80211_AC_BK;
250	case 2:
251	return IEEE80211_AC_VI;
252	case 3:
253	return IEEE80211_AC_VO;
254	}
255	}
256
257	static u8 ieee80211_wmm_aci_aifsn(int aifsn, bool acm, int aci)
258	{
259	u8 ret;
260
261	ret = aifsn & 0x0f;
262	if (acm)
263	ret |= 0x10;
264	ret |= (aci << 5) & 0x60;
265	return ret;
266	}
267
268	static u8 ieee80211_wmm_ecw(u16 cw_min, u16 cw_max)
269	{
270	return ((ilog2(cw_min + 1) << 0x0) & 0x0f) |
271	((ilog2(cw_max + 1) << 0x4) & 0xf0);
272	}
273
274	static void ieee80211_tdls_add_wmm_param_ie(struct ieee80211_sub_if_data *sdata,
275	struct sk_buff *skb)
276	{
277	struct ieee80211_wmm_param_ie *wmm;
278	struct ieee80211_tx_queue_params *txq;
279	int i;
280
281	wmm = skb_put_zero(skb, len: sizeof(*wmm));
282
283	wmm->element_id = WLAN_EID_VENDOR_SPECIFIC;
284	wmm->len = sizeof(*wmm) - 2;
285
286	wmm->oui[0] = 0x00; /* Microsoft OUI 00:50:F2 */
287	wmm->oui[1] = 0x50;
288	wmm->oui[2] = 0xf2;
289	wmm->oui_type = 2; /* WME */
290	wmm->oui_subtype = 1; /* WME param */
291	wmm->version = 1; /* WME ver */
292	wmm->qos_info = 0; /* U-APSD not in use */
293
294	/*
295	* Use the EDCA parameters defined for the BSS, or default if the AP
296	* doesn't support it, as mandated by 802.11-2012 section 10.22.4
297	*/
298	for (i = 0; i < IEEE80211_NUM_ACS; i++) {
299	txq = &sdata->deflink.tx_conf[ieee80211_ac_from_wmm(ac: i)];
300	wmm->ac[i].aci_aifsn = ieee80211_wmm_aci_aifsn(aifsn: txq->aifs,
301	acm: txq->acm, aci: i);
302	wmm->ac[i].cw = ieee80211_wmm_ecw(cw_min: txq->cw_min, cw_max: txq->cw_max);
303	wmm->ac[i].txop_limit = cpu_to_le16(txq->txop);
304	}
305	}
306
307	static void
308	ieee80211_tdls_chandef_vht_upgrade(struct ieee80211_sub_if_data *sdata,
309	struct sta_info *sta)
310	{
311	/* IEEE802.11ac-2013 Table E-4 */
312	static const u16 centers_80mhz[] = { 5210, 5290, 5530, 5610, 5690, 5775 };
313	struct cfg80211_chan_def uc = sta->tdls_chandef;
314	enum nl80211_chan_width max_width =
315	ieee80211_sta_cap_chan_bw(link_sta: &sta->deflink);
316	int i;
317
318	/* only support upgrading non-narrow channels up to 80Mhz */
319	if (max_width == NL80211_CHAN_WIDTH_5 ||
320	max_width == NL80211_CHAN_WIDTH_10)
321	return;
322
323	if (max_width > NL80211_CHAN_WIDTH_80)
324	max_width = NL80211_CHAN_WIDTH_80;
325
326	if (uc.width >= max_width)
327	return;
328	/*
329	* Channel usage constrains in the IEEE802.11ac-2013 specification only
330	* allow expanding a 20MHz channel to 80MHz in a single way. In
331	* addition, there are no 40MHz allowed channels that are not part of
332	* the allowed 80MHz range in the 5GHz spectrum (the relevant one here).
333	*/
334	for (i = 0; i < ARRAY_SIZE(centers_80mhz); i++)
335	if (abs(uc.chan->center_freq - centers_80mhz[i]) <= 30) {
336	uc.center_freq1 = centers_80mhz[i];
337	uc.center_freq2 = 0;
338	uc.width = NL80211_CHAN_WIDTH_80;
339	break;
340	}
341
342	if (!uc.center_freq1)
343	return;
344
345	/* proceed to downgrade the chandef until usable or the same as AP BW */
346	while (uc.width > max_width ||
347	(uc.width > sta->tdls_chandef.width &&
348	!cfg80211_reg_can_beacon_relax(wiphy: sdata->local->hw.wiphy, chandef: &uc,
349	iftype: sdata->wdev.iftype)))
350	ieee80211_chandef_downgrade(c: &uc);
351
352	if (!cfg80211_chandef_identical(chandef1: &uc, chandef2: &sta->tdls_chandef)) {
353	tdls_dbg(sdata, "TDLS ch width upgraded %d -> %d\n",
354	sta->tdls_chandef.width, uc.width);
355
356	/*
357	* the station is not yet authorized when BW upgrade is done,
358	* locking is not required
359	*/
360	sta->tdls_chandef = uc;
361	}
362	}
363
364	static void
365	ieee80211_tdls_add_setup_start_ies(struct ieee80211_link_data *link,
366	struct sk_buff *skb, const u8 *peer,
367	u8 action_code, bool initiator,
368	const u8 *extra_ies, size_t extra_ies_len)
369	{
370	struct ieee80211_sub_if_data *sdata = link->sdata;
371	struct ieee80211_supported_band *sband;
372	struct ieee80211_local *local = sdata->local;
373	struct ieee80211_sta_ht_cap ht_cap;
374	struct ieee80211_sta_vht_cap vht_cap;
375	const struct ieee80211_sta_he_cap *he_cap;
376	const struct ieee80211_sta_eht_cap *eht_cap;
377	struct sta_info *sta = NULL;
378	size_t offset = 0, noffset;
379	u8 *pos;
380
381	sband = ieee80211_get_link_sband(link);
382	if (WARN_ON_ONCE(!sband))
383	return;
384
385	ieee80211_add_srates_ie(sdata, skb, need_basic: false, band: sband->band);
386	ieee80211_add_ext_srates_ie(sdata, skb, need_basic: false, band: sband->band);
387	ieee80211_tdls_add_supp_channels(sdata, skb);
388
389	/* add any custom IEs that go before Extended Capabilities */
390	if (extra_ies_len) {
391	static const u8 before_ext_cap[] = {
392	WLAN_EID_SUPP_RATES,
393	WLAN_EID_COUNTRY,
394	WLAN_EID_EXT_SUPP_RATES,
395	WLAN_EID_SUPPORTED_CHANNELS,
396	WLAN_EID_RSN,
397	};
398	noffset = ieee80211_ie_split(ies: extra_ies, ielen: extra_ies_len,
399	ids: before_ext_cap,
400	ARRAY_SIZE(before_ext_cap),
401	offset);
402	skb_put_data(skb, data: extra_ies + offset, len: noffset - offset);
403	offset = noffset;
404	}
405
406	ieee80211_tdls_add_ext_capab(link, skb);
407
408	/* add the QoS element if we support it */
409	if (local->hw.queues >= IEEE80211_NUM_ACS &&
410	action_code != WLAN_PUB_ACTION_TDLS_DISCOVER_RES)
411	ieee80211_add_wmm_info_ie(buf: skb_put(skb, len: 9), qosinfo: 0); /* no U-APSD */
412
413	/* add any custom IEs that go before HT capabilities */
414	if (extra_ies_len) {
415	static const u8 before_ht_cap[] = {
416	WLAN_EID_SUPP_RATES,
417	WLAN_EID_COUNTRY,
418	WLAN_EID_EXT_SUPP_RATES,
419	WLAN_EID_SUPPORTED_CHANNELS,
420	WLAN_EID_RSN,
421	WLAN_EID_EXT_CAPABILITY,
422	WLAN_EID_QOS_CAPA,
423	WLAN_EID_FAST_BSS_TRANSITION,
424	WLAN_EID_TIMEOUT_INTERVAL,
425	WLAN_EID_SUPPORTED_REGULATORY_CLASSES,
426	};
427	noffset = ieee80211_ie_split(ies: extra_ies, ielen: extra_ies_len,
428	ids: before_ht_cap,
429	ARRAY_SIZE(before_ht_cap),
430	offset);
431	skb_put_data(skb, data: extra_ies + offset, len: noffset - offset);
432	offset = noffset;
433	}
434
435	/* we should have the peer STA if we're already responding */
436	if (action_code == WLAN_TDLS_SETUP_RESPONSE) {
437	sta = sta_info_get(sdata, addr: peer);
438	if (WARN_ON_ONCE(!sta))
439	return;
440
441	sta->tdls_chandef = link->conf->chandef;
442	}
443
444	ieee80211_tdls_add_oper_classes(link, skb);
445
446	/*
447	* with TDLS we can switch channels, and HT-caps are not necessarily
448	* the same on all bands. The specification limits the setup to a
449	* single HT-cap, so use the current band for now.
450	*/
451	memcpy(&ht_cap, &sband->ht_cap, sizeof(ht_cap));
452
453	if ((action_code == WLAN_TDLS_SETUP_REQUEST ||
454	action_code == WLAN_PUB_ACTION_TDLS_DISCOVER_RES) &&
455	ht_cap.ht_supported) {
456	ieee80211_apply_htcap_overrides(sdata, ht_cap: &ht_cap);
457
458	/* disable SMPS in TDLS initiator */
459	ht_cap.cap |= WLAN_HT_CAP_SM_PS_DISABLED
460	<< IEEE80211_HT_CAP_SM_PS_SHIFT;
461
462	pos = skb_put(skb, len: sizeof(struct ieee80211_ht_cap) + 2);
463	ieee80211_ie_build_ht_cap(pos, ht_cap: &ht_cap, cap: ht_cap.cap);
464	} else if (action_code == WLAN_TDLS_SETUP_RESPONSE &&
465	ht_cap.ht_supported && sta->sta.deflink.ht_cap.ht_supported) {
466	/* the peer caps are already intersected with our own */
467	memcpy(&ht_cap, &sta->sta.deflink.ht_cap, sizeof(ht_cap));
468
469	pos = skb_put(skb, len: sizeof(struct ieee80211_ht_cap) + 2);
470	ieee80211_ie_build_ht_cap(pos, ht_cap: &ht_cap, cap: ht_cap.cap);
471	}
472
473	if (ht_cap.ht_supported &&
474	(ht_cap.cap & IEEE80211_HT_CAP_SUP_WIDTH_20_40))
475	ieee80211_tdls_add_bss_coex_ie(skb);
476
477	ieee80211_tdls_add_link_ie(link, skb, peer, initiator);
478
479	/* add any custom IEs that go before VHT capabilities */
480	if (extra_ies_len) {
481	static const u8 before_vht_cap[] = {
482	WLAN_EID_SUPP_RATES,
483	WLAN_EID_COUNTRY,
484	WLAN_EID_EXT_SUPP_RATES,
485	WLAN_EID_SUPPORTED_CHANNELS,
486	WLAN_EID_RSN,
487	WLAN_EID_EXT_CAPABILITY,
488	WLAN_EID_QOS_CAPA,
489	WLAN_EID_FAST_BSS_TRANSITION,
490	WLAN_EID_TIMEOUT_INTERVAL,
491	WLAN_EID_SUPPORTED_REGULATORY_CLASSES,
492	WLAN_EID_MULTI_BAND,
493	};
494	noffset = ieee80211_ie_split(ies: extra_ies, ielen: extra_ies_len,
495	ids: before_vht_cap,
496	ARRAY_SIZE(before_vht_cap),
497	offset);
498	skb_put_data(skb, data: extra_ies + offset, len: noffset - offset);
499	offset = noffset;
500	}
501
502	/* add AID if VHT, HE or EHT capabilities supported */
503	memcpy(&vht_cap, &sband->vht_cap, sizeof(vht_cap));
504	he_cap = ieee80211_get_he_iftype_cap_vif(sband, vif: &sdata->vif);
505	eht_cap = ieee80211_get_eht_iftype_cap_vif(sband, vif: &sdata->vif);
506	if ((vht_cap.vht_supported || he_cap || eht_cap) &&
507	(action_code == WLAN_TDLS_SETUP_REQUEST ||
508	action_code == WLAN_TDLS_SETUP_RESPONSE))
509	ieee80211_tdls_add_aid(sdata, skb);
510
511	/* build the VHT-cap similarly to the HT-cap */
512	if ((action_code == WLAN_TDLS_SETUP_REQUEST ||
513	action_code == WLAN_PUB_ACTION_TDLS_DISCOVER_RES) &&
514	vht_cap.vht_supported) {
515	ieee80211_apply_vhtcap_overrides(sdata, vht_cap: &vht_cap);
516
517	pos = skb_put(skb, len: sizeof(struct ieee80211_vht_cap) + 2);
518	ieee80211_ie_build_vht_cap(pos, vht_cap: &vht_cap, cap: vht_cap.cap);
519	} else if (action_code == WLAN_TDLS_SETUP_RESPONSE &&
520	vht_cap.vht_supported && sta->sta.deflink.vht_cap.vht_supported) {
521	/* the peer caps are already intersected with our own */
522	memcpy(&vht_cap, &sta->sta.deflink.vht_cap, sizeof(vht_cap));
523
524	pos = skb_put(skb, len: sizeof(struct ieee80211_vht_cap) + 2);
525	ieee80211_ie_build_vht_cap(pos, vht_cap: &vht_cap, cap: vht_cap.cap);
526
527	/*
528	* if both peers support WIDER_BW, we can expand the chandef to
529	* a wider compatible one, up to 80MHz
530	*/
531	if (test_sta_flag(sta, flag: WLAN_STA_TDLS_WIDER_BW))
532	ieee80211_tdls_chandef_vht_upgrade(sdata, sta);
533	}
534
535	/* add any custom IEs that go before HE capabilities */
536	if (extra_ies_len) {
537	static const u8 before_he_cap[] = {
538	WLAN_EID_EXTENSION,
539	WLAN_EID_EXT_FILS_REQ_PARAMS,
540	WLAN_EID_AP_CSN,
541	};
542	noffset = ieee80211_ie_split(ies: extra_ies, ielen: extra_ies_len,
543	ids: before_he_cap,
544	ARRAY_SIZE(before_he_cap),
545	offset);
546	skb_put_data(skb, data: extra_ies + offset, len: noffset - offset);
547	offset = noffset;
548	}
549
550	/* build the HE-cap from sband */
551	if (he_cap &&
552	(action_code == WLAN_TDLS_SETUP_REQUEST ||
553	action_code == WLAN_TDLS_SETUP_RESPONSE ||
554	action_code == WLAN_PUB_ACTION_TDLS_DISCOVER_RES)) {
555	__le16 he_6ghz_capa;
556	u8 cap_size;
557
558	cap_size =
559	2 + 1 + sizeof(he_cap->he_cap_elem) +
560	ieee80211_he_mcs_nss_size(he_cap: &he_cap->he_cap_elem) +
561	ieee80211_he_ppe_size(ppe_thres_hdr: he_cap->ppe_thres[0],
562	phy_cap_info: he_cap->he_cap_elem.phy_cap_info);
563	pos = skb_put(skb, len: cap_size);
564	pos = ieee80211_ie_build_he_cap(disable_flags: 0, pos, he_cap, end: pos + cap_size);
565
566	/* Build HE 6Ghz capa IE from sband */
567	if (sband->band == NL80211_BAND_6GHZ) {
568	cap_size = 2 + 1 + sizeof(struct ieee80211_he_6ghz_capa);
569	pos = skb_put(skb, len: cap_size);
570	he_6ghz_capa =
571	ieee80211_get_he_6ghz_capa_vif(sband, vif: &sdata->vif);
572	pos = ieee80211_write_he_6ghz_cap(pos, cap: he_6ghz_capa,
573	end: pos + cap_size);
574	}
575	}
576
577	/* add any custom IEs that go before EHT capabilities */
578	if (extra_ies_len) {
579	static const u8 before_he_cap[] = {
580	WLAN_EID_EXTENSION,
581	WLAN_EID_EXT_FILS_REQ_PARAMS,
582	WLAN_EID_AP_CSN,
583	};
584
585	noffset = ieee80211_ie_split(ies: extra_ies, ielen: extra_ies_len,
586	ids: before_he_cap,
587	ARRAY_SIZE(before_he_cap),
588	offset);
589	skb_put_data(skb, data: extra_ies + offset, len: noffset - offset);
590	offset = noffset;
591	}
592
593	/* build the EHT-cap from sband */
594	if (he_cap && eht_cap &&
595	(action_code == WLAN_TDLS_SETUP_REQUEST ||
596	action_code == WLAN_TDLS_SETUP_RESPONSE ||
597	action_code == WLAN_PUB_ACTION_TDLS_DISCOVER_RES)) {
598	u8 cap_size;
599
600	cap_size =
601	2 + 1 + sizeof(eht_cap->eht_cap_elem) +
602	ieee80211_eht_mcs_nss_size(he_cap: &he_cap->he_cap_elem,
603	eht_cap: &eht_cap->eht_cap_elem, from_ap: false) +
604	ieee80211_eht_ppe_size(ppe_thres_hdr: eht_cap->eht_ppe_thres[0],
605	phy_cap_info: eht_cap->eht_cap_elem.phy_cap_info);
606	pos = skb_put(skb, len: cap_size);
607	ieee80211_ie_build_eht_cap(pos, he_cap, eht_cap, end: pos + cap_size, for_ap: false);
608	}
609
610	/* add any remaining IEs */
611	if (extra_ies_len) {
612	noffset = extra_ies_len;
613	skb_put_data(skb, data: extra_ies + offset, len: noffset - offset);
614	}
615
616	}
617
618	static void
619	ieee80211_tdls_add_setup_cfm_ies(struct ieee80211_link_data *link,
620	struct sk_buff *skb, const u8 *peer,
621	bool initiator, const u8 *extra_ies,
622	size_t extra_ies_len)
623	{
624	struct ieee80211_sub_if_data *sdata = link->sdata;
625	struct ieee80211_local *local = sdata->local;
626	size_t offset = 0, noffset;
627	struct sta_info *sta, *ap_sta;
628	struct ieee80211_supported_band *sband;
629	u8 *pos;
630
631	sband = ieee80211_get_link_sband(link);
632	if (WARN_ON_ONCE(!sband))
633	return;
634
635	sta = sta_info_get(sdata, addr: peer);
636	ap_sta = sta_info_get(sdata, addr: sdata->vif.cfg.ap_addr);
637
638	if (WARN_ON_ONCE(!sta || !ap_sta))
639	return;
640
641	sta->tdls_chandef = link->conf->chandef;
642
643	/* add any custom IEs that go before the QoS IE */
644	if (extra_ies_len) {
645	static const u8 before_qos[] = {
646	WLAN_EID_RSN,
647	};
648	noffset = ieee80211_ie_split(ies: extra_ies, ielen: extra_ies_len,
649	ids: before_qos,
650	ARRAY_SIZE(before_qos),
651	offset);
652	skb_put_data(skb, data: extra_ies + offset, len: noffset - offset);
653	offset = noffset;
654	}
655
656	/* add the QoS param IE if both the peer and we support it */
657	if (local->hw.queues >= IEEE80211_NUM_ACS && sta->sta.wme)
658	ieee80211_tdls_add_wmm_param_ie(sdata, skb);
659
660	/* add any custom IEs that go before HT operation */
661	if (extra_ies_len) {
662	static const u8 before_ht_op[] = {
663	WLAN_EID_RSN,
664	WLAN_EID_QOS_CAPA,
665	WLAN_EID_FAST_BSS_TRANSITION,
666	WLAN_EID_TIMEOUT_INTERVAL,
667	};
668	noffset = ieee80211_ie_split(ies: extra_ies, ielen: extra_ies_len,
669	ids: before_ht_op,
670	ARRAY_SIZE(before_ht_op),
671	offset);
672	skb_put_data(skb, data: extra_ies + offset, len: noffset - offset);
673	offset = noffset;
674	}
675
676	/*
677	* if HT support is only added in TDLS, we need an HT-operation IE.
678	* add the IE as required by IEEE802.11-2012 9.23.3.2.
679	*/
680	if (!ap_sta->sta.deflink.ht_cap.ht_supported && sta->sta.deflink.ht_cap.ht_supported) {
681	u16 prot = IEEE80211_HT_OP_MODE_PROTECTION_NONHT_MIXED |
682	IEEE80211_HT_OP_MODE_NON_GF_STA_PRSNT |
683	IEEE80211_HT_OP_MODE_NON_HT_STA_PRSNT;
684
685	pos = skb_put(skb, len: 2 + sizeof(struct ieee80211_ht_operation));
686	ieee80211_ie_build_ht_oper(pos, ht_cap: &sta->sta.deflink.ht_cap,
687	chandef: &link->conf->chandef, prot_mode: prot,
688	rifs_mode: true);
689	}
690
691	ieee80211_tdls_add_link_ie(link, skb, peer, initiator);
692
693	/* only include VHT-operation if not on the 2.4GHz band */
694	if (sband->band != NL80211_BAND_2GHZ &&
695	sta->sta.deflink.vht_cap.vht_supported) {
696	/*
697	* if both peers support WIDER_BW, we can expand the chandef to
698	* a wider compatible one, up to 80MHz
699	*/
700	if (test_sta_flag(sta, flag: WLAN_STA_TDLS_WIDER_BW))
701	ieee80211_tdls_chandef_vht_upgrade(sdata, sta);
702
703	pos = skb_put(skb, len: 2 + sizeof(struct ieee80211_vht_operation));
704	ieee80211_ie_build_vht_oper(pos, vht_cap: &sta->sta.deflink.vht_cap,
705	chandef: &sta->tdls_chandef);
706	}
707
708	/* add any remaining IEs */
709	if (extra_ies_len) {
710	noffset = extra_ies_len;
711	skb_put_data(skb, data: extra_ies + offset, len: noffset - offset);
712	}
713	}
714
715	static void
716	ieee80211_tdls_add_chan_switch_req_ies(struct ieee80211_link_data *link,
717	struct sk_buff *skb, const u8 *peer,
718	bool initiator, const u8 *extra_ies,
719	size_t extra_ies_len, u8 oper_class,
720	struct cfg80211_chan_def *chandef)
721	{
722	struct ieee80211_tdls_data *tf;
723	size_t offset = 0, noffset;
724
725	if (WARN_ON_ONCE(!chandef))
726	return;
727
728	tf = (void *)skb->data;
729	tf->u.chan_switch_req.target_channel =
730	ieee80211_frequency_to_channel(freq: chandef->chan->center_freq);
731	tf->u.chan_switch_req.oper_class = oper_class;
732
733	if (extra_ies_len) {
734	static const u8 before_lnkie[] = {
735	WLAN_EID_SECONDARY_CHANNEL_OFFSET,
736	};
737	noffset = ieee80211_ie_split(ies: extra_ies, ielen: extra_ies_len,
738	ids: before_lnkie,
739	ARRAY_SIZE(before_lnkie),
740	offset);
741	skb_put_data(skb, data: extra_ies + offset, len: noffset - offset);
742	offset = noffset;
743	}
744
745	ieee80211_tdls_add_link_ie(link, skb, peer, initiator);
746
747	/* add any remaining IEs */
748	if (extra_ies_len) {
749	noffset = extra_ies_len;
750	skb_put_data(skb, data: extra_ies + offset, len: noffset - offset);
751	}
752	}
753
754	static void
755	ieee80211_tdls_add_chan_switch_resp_ies(struct ieee80211_link_data *link,
756	struct sk_buff *skb, const u8 *peer,
757	u16 status_code, bool initiator,
758	const u8 *extra_ies,
759	size_t extra_ies_len)
760	{
761	if (status_code == 0)
762	ieee80211_tdls_add_link_ie(link, skb, peer, initiator);
763
764	if (extra_ies_len)
765	skb_put_data(skb, data: extra_ies, len: extra_ies_len);
766	}
767
768	static void ieee80211_tdls_add_ies(struct ieee80211_link_data *link,
769	struct sk_buff *skb, const u8 *peer,
770	u8 action_code, u16 status_code,
771	bool initiator, const u8 *extra_ies,
772	size_t extra_ies_len, u8 oper_class,
773	struct cfg80211_chan_def *chandef)
774	{
775	switch (action_code) {
776	case WLAN_TDLS_SETUP_REQUEST:
777	case WLAN_TDLS_SETUP_RESPONSE:
778	case WLAN_PUB_ACTION_TDLS_DISCOVER_RES:
779	if (status_code == 0)
780	ieee80211_tdls_add_setup_start_ies(link,
781	skb, peer,
782	action_code,
783	initiator,
784	extra_ies,
785	extra_ies_len);
786	break;
787	case WLAN_TDLS_SETUP_CONFIRM:
788	if (status_code == 0)
789	ieee80211_tdls_add_setup_cfm_ies(link, skb, peer,
790	initiator, extra_ies,
791	extra_ies_len);
792	break;
793	case WLAN_TDLS_TEARDOWN:
794	case WLAN_TDLS_DISCOVERY_REQUEST:
795	if (extra_ies_len)
796	skb_put_data(skb, data: extra_ies, len: extra_ies_len);
797	if (status_code == 0 || action_code == WLAN_TDLS_TEARDOWN)
798	ieee80211_tdls_add_link_ie(link, skb,
799	peer, initiator);
800	break;
801	case WLAN_TDLS_CHANNEL_SWITCH_REQUEST:
802	ieee80211_tdls_add_chan_switch_req_ies(link, skb, peer,
803	initiator, extra_ies,
804	extra_ies_len,
805	oper_class, chandef);
806	break;
807	case WLAN_TDLS_CHANNEL_SWITCH_RESPONSE:
808	ieee80211_tdls_add_chan_switch_resp_ies(link, skb, peer,
809	status_code,
810	initiator, extra_ies,
811	extra_ies_len);
812	break;
813	}
814
815	}
816
817	static int
818	ieee80211_prep_tdls_encap_data(struct wiphy *wiphy, struct net_device *dev,
819	struct ieee80211_link_data *link,
820	const u8 *peer, u8 action_code, u8 dialog_token,
821	u16 status_code, struct sk_buff *skb)
822	{
823	struct ieee80211_sub_if_data *sdata = IEEE80211_DEV_TO_SUB_IF(dev);
824	struct ieee80211_tdls_data *tf;
825
826	tf = skb_put(skb, offsetof(struct ieee80211_tdls_data, u));
827
828	memcpy(tf->da, peer, ETH_ALEN);
829	memcpy(tf->sa, sdata->vif.addr, ETH_ALEN);
830	tf->ether_type = cpu_to_be16(ETH_P_TDLS);
831	tf->payload_type = WLAN_TDLS_SNAP_RFTYPE;
832
833	/* network header is after the ethernet header */
834	skb_set_network_header(skb, ETH_HLEN);
835
836	switch (action_code) {
837	case WLAN_TDLS_SETUP_REQUEST:
838	tf->category = WLAN_CATEGORY_TDLS;
839	tf->action_code = WLAN_TDLS_SETUP_REQUEST;
840
841	skb_put(skb, len: sizeof(tf->u.setup_req));
842	tf->u.setup_req.dialog_token = dialog_token;
843	tf->u.setup_req.capability =
844	cpu_to_le16(ieee80211_get_tdls_sta_capab(link,
845	status_code));
846	break;
847	case WLAN_TDLS_SETUP_RESPONSE:
848	tf->category = WLAN_CATEGORY_TDLS;
849	tf->action_code = WLAN_TDLS_SETUP_RESPONSE;
850
851	skb_put(skb, len: sizeof(tf->u.setup_resp));
852	tf->u.setup_resp.status_code = cpu_to_le16(status_code);
853	tf->u.setup_resp.dialog_token = dialog_token;
854	tf->u.setup_resp.capability =
855	cpu_to_le16(ieee80211_get_tdls_sta_capab(link,
856	status_code));
857	break;
858	case WLAN_TDLS_SETUP_CONFIRM:
859	tf->category = WLAN_CATEGORY_TDLS;
860	tf->action_code = WLAN_TDLS_SETUP_CONFIRM;
861
862	skb_put(skb, len: sizeof(tf->u.setup_cfm));
863	tf->u.setup_cfm.status_code = cpu_to_le16(status_code);
864	tf->u.setup_cfm.dialog_token = dialog_token;
865	break;
866	case WLAN_TDLS_TEARDOWN:
867	tf->category = WLAN_CATEGORY_TDLS;
868	tf->action_code = WLAN_TDLS_TEARDOWN;
869
870	skb_put(skb, len: sizeof(tf->u.teardown));
871	tf->u.teardown.reason_code = cpu_to_le16(status_code);
872	break;
873	case WLAN_TDLS_DISCOVERY_REQUEST:
874	tf->category = WLAN_CATEGORY_TDLS;
875	tf->action_code = WLAN_TDLS_DISCOVERY_REQUEST;
876
877	skb_put(skb, len: sizeof(tf->u.discover_req));
878	tf->u.discover_req.dialog_token = dialog_token;
879	break;
880	case WLAN_TDLS_CHANNEL_SWITCH_REQUEST:
881	tf->category = WLAN_CATEGORY_TDLS;
882	tf->action_code = WLAN_TDLS_CHANNEL_SWITCH_REQUEST;
883
884	skb_put(skb, len: sizeof(tf->u.chan_switch_req));
885	break;
886	case WLAN_TDLS_CHANNEL_SWITCH_RESPONSE:
887	tf->category = WLAN_CATEGORY_TDLS;
888	tf->action_code = WLAN_TDLS_CHANNEL_SWITCH_RESPONSE;
889
890	skb_put(skb, len: sizeof(tf->u.chan_switch_resp));
891	tf->u.chan_switch_resp.status_code = cpu_to_le16(status_code);
892	break;
893	default:
894	return -EINVAL;
895	}
896
897	return 0;
898	}
899
900	static int
901	ieee80211_prep_tdls_direct(struct wiphy *wiphy, struct net_device *dev,
902	const u8 *peer, struct ieee80211_link_data *link,
903	u8 action_code, u8 dialog_token,
904	u16 status_code, struct sk_buff *skb)
905	{
906	struct ieee80211_sub_if_data *sdata = IEEE80211_DEV_TO_SUB_IF(dev);
907	struct ieee80211_mgmt *mgmt;
908
909	mgmt = skb_put_zero(skb, len: 24);
910	memcpy(mgmt->da, peer, ETH_ALEN);
911	memcpy(mgmt->sa, sdata->vif.addr, ETH_ALEN);
912	memcpy(mgmt->bssid, link->u.mgd.bssid, ETH_ALEN);
913	mgmt->frame_control = cpu_to_le16(IEEE80211_FTYPE_MGMT |
914	IEEE80211_STYPE_ACTION);
915
916	switch (action_code) {
917	case WLAN_PUB_ACTION_TDLS_DISCOVER_RES:
918	skb_put(skb, len: 1 + sizeof(mgmt->u.action.u.tdls_discover_resp));
919	mgmt->u.action.category = WLAN_CATEGORY_PUBLIC;
920	mgmt->u.action.u.tdls_discover_resp.action_code =
921	WLAN_PUB_ACTION_TDLS_DISCOVER_RES;
922	mgmt->u.action.u.tdls_discover_resp.dialog_token =
923	dialog_token;
924	mgmt->u.action.u.tdls_discover_resp.capability =
925	cpu_to_le16(ieee80211_get_tdls_sta_capab(link,
926	status_code));
927	break;
928	default:
929	return -EINVAL;
930	}
931
932	return 0;
933	}
934
935	static struct sk_buff *
936	ieee80211_tdls_build_mgmt_packet_data(struct ieee80211_sub_if_data *sdata,
937	const u8 *peer, int link_id,
938	u8 action_code, u8 dialog_token,
939	u16 status_code, bool initiator,
940	const u8 *extra_ies, size_t extra_ies_len,
941	u8 oper_class,
942	struct cfg80211_chan_def *chandef)
943	{
944	struct ieee80211_local *local = sdata->local;
945	struct sk_buff *skb;
946	int ret;
947	struct ieee80211_link_data *link;
948
949	link_id = link_id >= 0 ? link_id : 0;
950	rcu_read_lock();
951	link = rcu_dereference(sdata->link[link_id]);
952	if (WARN_ON(!link))
953	goto unlock;
954
955	skb = netdev_alloc_skb(dev: sdata->dev,
956	length: local->hw.extra_tx_headroom +
957	max(sizeof(struct ieee80211_mgmt),
958	sizeof(struct ieee80211_tdls_data)) +
959	50 + /* supported rates */
960	10 + /* ext capab */
961	26 + /* max(WMM-info, WMM-param) */
962	2 + max(sizeof(struct ieee80211_ht_cap),
963	sizeof(struct ieee80211_ht_operation)) +
964	2 + max(sizeof(struct ieee80211_vht_cap),
965	sizeof(struct ieee80211_vht_operation)) +
966	2 + 1 + sizeof(struct ieee80211_he_cap_elem) +
967	sizeof(struct ieee80211_he_mcs_nss_supp) +
968	IEEE80211_HE_PPE_THRES_MAX_LEN +
969	2 + 1 + sizeof(struct ieee80211_he_6ghz_capa) +
970	2 + 1 + sizeof(struct ieee80211_eht_cap_elem) +
971	sizeof(struct ieee80211_eht_mcs_nss_supp) +
972	IEEE80211_EHT_PPE_THRES_MAX_LEN +
973	50 + /* supported channels */
974	3 + /* 40/20 BSS coex */
975	4 + /* AID */
976	4 + /* oper classes */
977	extra_ies_len +
978	sizeof(struct ieee80211_tdls_lnkie));
979	if (!skb)
980	goto unlock;
981
982	skb_reserve(skb, len: local->hw.extra_tx_headroom);
983
984	switch (action_code) {
985	case WLAN_TDLS_SETUP_REQUEST:
986	case WLAN_TDLS_SETUP_RESPONSE:
987	case WLAN_TDLS_SETUP_CONFIRM:
988	case WLAN_TDLS_TEARDOWN:
989	case WLAN_TDLS_DISCOVERY_REQUEST:
990	case WLAN_TDLS_CHANNEL_SWITCH_REQUEST:
991	case WLAN_TDLS_CHANNEL_SWITCH_RESPONSE:
992	ret = ieee80211_prep_tdls_encap_data(wiphy: local->hw.wiphy,
993	dev: sdata->dev, link, peer,
994	action_code, dialog_token,
995	status_code, skb);
996	break;
997	case WLAN_PUB_ACTION_TDLS_DISCOVER_RES:
998	ret = ieee80211_prep_tdls_direct(wiphy: local->hw.wiphy, dev: sdata->dev,
999	peer, link, action_code,
1000	dialog_token, status_code,
1001	skb);
1002	break;
1003	default:
1004	ret = -ENOTSUPP;
1005	break;
1006	}
1007
1008	if (ret < 0)
1009	goto fail;
1010
1011	ieee80211_tdls_add_ies(link, skb, peer, action_code, status_code,
1012	initiator, extra_ies, extra_ies_len, oper_class,
1013	chandef);
1014	rcu_read_unlock();
1015	return skb;
1016
1017	fail:
1018	dev_kfree_skb(skb);
1019	unlock:
1020	rcu_read_unlock();
1021	return NULL;
1022	}
1023
1024	static int
1025	ieee80211_tdls_prep_mgmt_packet(struct wiphy *wiphy, struct net_device *dev,
1026	const u8 *peer, int link_id,
1027	u8 action_code, u8 dialog_token,
1028	u16 status_code, u32 peer_capability,
1029	bool initiator, const u8 *extra_ies,
1030	size_t extra_ies_len, u8 oper_class,
1031	struct cfg80211_chan_def *chandef)
1032	{
1033	struct ieee80211_sub_if_data *sdata = IEEE80211_DEV_TO_SUB_IF(dev);
1034	struct sk_buff *skb = NULL;
1035	struct sta_info *sta;
1036	u32 flags = 0;
1037	int ret = 0;
1038
1039	rcu_read_lock();
1040	sta = sta_info_get(sdata, addr: peer);
1041
1042	/* infer the initiator if we can, to support old userspace */
1043	switch (action_code) {
1044	case WLAN_TDLS_SETUP_REQUEST:
1045	if (sta) {
1046	set_sta_flag(sta, flag: WLAN_STA_TDLS_INITIATOR);
1047	sta->sta.tdls_initiator = false;
1048	}
1049	fallthrough;
1050	case WLAN_TDLS_SETUP_CONFIRM:
1051	case WLAN_TDLS_DISCOVERY_REQUEST:
1052	initiator = true;
1053	break;
1054	case WLAN_TDLS_SETUP_RESPONSE:
1055	/*
1056	* In some testing scenarios, we send a request and response.
1057	* Make the last packet sent take effect for the initiator
1058	* value.
1059	*/
1060	if (sta) {
1061	clear_sta_flag(sta, flag: WLAN_STA_TDLS_INITIATOR);
1062	sta->sta.tdls_initiator = true;
1063	}
1064	fallthrough;
1065	case WLAN_PUB_ACTION_TDLS_DISCOVER_RES:
1066	initiator = false;
1067	break;
1068	case WLAN_TDLS_TEARDOWN:
1069	case WLAN_TDLS_CHANNEL_SWITCH_REQUEST:
1070	case WLAN_TDLS_CHANNEL_SWITCH_RESPONSE:
1071	/* any value is ok */
1072	break;
1073	default:
1074	ret = -ENOTSUPP;
1075	break;
1076	}
1077
1078	if (sta && test_sta_flag(sta, flag: WLAN_STA_TDLS_INITIATOR))
1079	initiator = true;
1080
1081	rcu_read_unlock();
1082	if (ret < 0)
1083	goto fail;
1084
1085	skb = ieee80211_tdls_build_mgmt_packet_data(sdata, peer,
1086	link_id, action_code,
1087	dialog_token, status_code,
1088	initiator, extra_ies,
1089	extra_ies_len, oper_class,
1090	chandef);
1091	if (!skb) {
1092	ret = -EINVAL;
1093	goto fail;
1094	}
1095
1096	if (action_code == WLAN_PUB_ACTION_TDLS_DISCOVER_RES) {
1097	ieee80211_tx_skb_tid(sdata, skb, tid: 7, link_id);
1098	return 0;
1099	}
1100
1101	/*
1102	* According to 802.11z: Setup req/resp are sent in AC_BK, otherwise
1103	* we should default to AC_VI.
1104	*/
1105	switch (action_code) {
1106	case WLAN_TDLS_SETUP_REQUEST:
1107	case WLAN_TDLS_SETUP_RESPONSE:
1108	skb->priority = 256 + 2;
1109	break;
1110	default:
1111	skb->priority = 256 + 5;
1112	break;
1113	}
1114
1115	/*
1116	* Set the WLAN_TDLS_TEARDOWN flag to indicate a teardown in progress.
1117	* Later, if no ACK is returned from peer, we will re-send the teardown
1118	* packet through the AP.
1119	*/
1120	if ((action_code == WLAN_TDLS_TEARDOWN) &&
1121	ieee80211_hw_check(&sdata->local->hw, REPORTS_TX_ACK_STATUS)) {
1122	bool try_resend; /* Should we keep skb for possible resend */
1123
1124	/* If not sending directly to peer - no point in keeping skb */
1125	rcu_read_lock();
1126	sta = sta_info_get(sdata, addr: peer);
1127	try_resend = sta && test_sta_flag(sta, flag: WLAN_STA_TDLS_PEER_AUTH);
1128	rcu_read_unlock();
1129
1130	spin_lock_bh(lock: &sdata->u.mgd.teardown_lock);
1131	if (try_resend && !sdata->u.mgd.teardown_skb) {
1132	/* Mark it as requiring TX status callback */
1133	flags |= IEEE80211_TX_CTL_REQ_TX_STATUS |
1134	IEEE80211_TX_INTFL_MLME_CONN_TX;
1135
1136	/*
1137	* skb is copied since mac80211 will later set
1138	* properties that might not be the same as the AP,
1139	* such as encryption, QoS, addresses, etc.
1140	*
1141	* No problem if skb_copy() fails, so no need to check.
1142	*/
1143	sdata->u.mgd.teardown_skb = skb_copy(skb, GFP_ATOMIC);
1144	sdata->u.mgd.orig_teardown_skb = skb;
1145	}
1146	spin_unlock_bh(lock: &sdata->u.mgd.teardown_lock);
1147	}
1148
1149	/* disable bottom halves when entering the Tx path */
1150	local_bh_disable();
1151	__ieee80211_subif_start_xmit(skb, dev, info_flags: flags,
1152	IEEE80211_TX_CTRL_MLO_LINK_UNSPEC, NULL);
1153	local_bh_enable();
1154
1155	return ret;
1156
1157	fail:
1158	dev_kfree_skb(skb);
1159	return ret;
1160	}
1161
1162	static int
1163	ieee80211_tdls_mgmt_setup(struct wiphy *wiphy, struct net_device *dev,
1164	const u8 *peer, int link_id,
1165	u8 action_code, u8 dialog_token,
1166	u16 status_code, u32 peer_capability, bool initiator,
1167	const u8 *extra_ies, size_t extra_ies_len)
1168	{
1169	struct ieee80211_sub_if_data *sdata = IEEE80211_DEV_TO_SUB_IF(dev);
1170	struct ieee80211_local *local = sdata->local;
1171	enum ieee80211_smps_mode smps_mode =
1172	sdata->deflink.u.mgd.driver_smps_mode;
1173	int ret;
1174
1175	/* don't support setup with forced SMPS mode that's not off */
1176	if (smps_mode != IEEE80211_SMPS_AUTOMATIC &&
1177	smps_mode != IEEE80211_SMPS_OFF) {
1178	tdls_dbg(sdata, "Aborting TDLS setup due to SMPS mode %d\n",
1179	smps_mode);
1180	return -ENOTSUPP;
1181	}
1182
1183	lockdep_assert_wiphy(local->hw.wiphy);
1184
1185	/* we don't support concurrent TDLS peer setups */
1186	if (!is_zero_ether_addr(addr: sdata->u.mgd.tdls_peer) &&
1187	!ether_addr_equal(addr1: sdata->u.mgd.tdls_peer, addr2: peer)) {
1188	ret = -EBUSY;
1189	goto out_unlock;
1190	}
1191
1192	/*
1193	* make sure we have a STA representing the peer so we drop or buffer
1194	* non-TDLS-setup frames to the peer. We can't send other packets
1195	* during setup through the AP path.
1196	* Allow error packets to be sent - sometimes we don't even add a STA
1197	* before failing the setup.
1198	*/
1199	if (status_code == 0) {
1200	rcu_read_lock();
1201	if (!sta_info_get(sdata, addr: peer)) {
1202	rcu_read_unlock();
1203	ret = -ENOLINK;
1204	goto out_unlock;
1205	}
1206	rcu_read_unlock();
1207	}
1208
1209	ieee80211_flush_queues(local, sdata, drop: false);
1210	memcpy(sdata->u.mgd.tdls_peer, peer, ETH_ALEN);
1211
1212	/* we cannot take the mutex while preparing the setup packet */
1213	ret = ieee80211_tdls_prep_mgmt_packet(wiphy, dev, peer,
1214	link_id, action_code,
1215	dialog_token, status_code,
1216	peer_capability, initiator,
1217	extra_ies, extra_ies_len, oper_class: 0,
1218	NULL);
1219	if (ret < 0) {
1220	eth_zero_addr(addr: sdata->u.mgd.tdls_peer);
1221	return ret;
1222	}
1223
1224	wiphy_delayed_work_queue(wiphy: sdata->local->hw.wiphy,
1225	dwork: &sdata->u.mgd.tdls_peer_del_work,
1226	TDLS_PEER_SETUP_TIMEOUT);
1227	return 0;
1228
1229	out_unlock:
1230	return ret;
1231	}
1232
1233	static int
1234	ieee80211_tdls_mgmt_teardown(struct wiphy *wiphy, struct net_device *dev,
1235	const u8 *peer, int link_id,
1236	u8 action_code, u8 dialog_token,
1237	u16 status_code, u32 peer_capability,
1238	bool initiator, const u8 *extra_ies,
1239	size_t extra_ies_len)
1240	{
1241	struct ieee80211_sub_if_data *sdata = IEEE80211_DEV_TO_SUB_IF(dev);
1242	struct ieee80211_local *local = sdata->local;
1243	struct sta_info *sta;
1244	int ret;
1245
1246	/*
1247	* No packets can be transmitted to the peer via the AP during setup -
1248	* the STA is set as a TDLS peer, but is not authorized.
1249	* During teardown, we prevent direct transmissions by stopping the
1250	* queues and flushing all direct packets.
1251	*/
1252	ieee80211_stop_vif_queues(local, sdata,
1253	reason: IEEE80211_QUEUE_STOP_REASON_TDLS_TEARDOWN);
1254	ieee80211_flush_queues(local, sdata, drop: false);
1255
1256	ret = ieee80211_tdls_prep_mgmt_packet(wiphy, dev, peer,
1257	link_id, action_code,
1258	dialog_token, status_code,
1259	peer_capability, initiator,
1260	extra_ies, extra_ies_len, oper_class: 0,
1261	NULL);
1262	if (ret < 0)
1263	sdata_err(sdata, "Failed sending TDLS teardown packet %d\n",
1264	ret);
1265
1266	/*
1267	* Remove the STA AUTH flag to force further traffic through the AP. If
1268	* the STA was unreachable, it was already removed.
1269	*/
1270	rcu_read_lock();
1271	sta = sta_info_get(sdata, addr: peer);
1272	if (sta)
1273	clear_sta_flag(sta, flag: WLAN_STA_TDLS_PEER_AUTH);
1274	rcu_read_unlock();
1275
1276	ieee80211_wake_vif_queues(local, sdata,
1277	reason: IEEE80211_QUEUE_STOP_REASON_TDLS_TEARDOWN);
1278
1279	return 0;
1280	}
1281
1282	int ieee80211_tdls_mgmt(struct wiphy *wiphy, struct net_device *dev,
1283	const u8 *peer, int link_id,
1284	u8 action_code, u8 dialog_token, u16 status_code,
1285	u32 peer_capability, bool initiator,
1286	const u8 *extra_ies, size_t extra_ies_len)
1287	{
1288	struct ieee80211_sub_if_data *sdata = IEEE80211_DEV_TO_SUB_IF(dev);
1289	int ret;
1290
1291	if (!(wiphy->flags & WIPHY_FLAG_SUPPORTS_TDLS))
1292	return -ENOTSUPP;
1293
1294	/* make sure we are in managed mode, and associated */
1295	if (sdata->vif.type != NL80211_IFTYPE_STATION ||
1296	!sdata->u.mgd.associated)
1297	return -EINVAL;
1298
1299	switch (action_code) {
1300	case WLAN_TDLS_SETUP_REQUEST:
1301	case WLAN_TDLS_SETUP_RESPONSE:
1302	ret = ieee80211_tdls_mgmt_setup(wiphy, dev, peer,
1303	link_id, action_code,
1304	dialog_token, status_code,
1305	peer_capability, initiator,
1306	extra_ies, extra_ies_len);
1307	break;
1308	case WLAN_TDLS_TEARDOWN:
1309	ret = ieee80211_tdls_mgmt_teardown(wiphy, dev, peer, link_id,
1310	action_code, dialog_token,
1311	status_code,
1312	peer_capability, initiator,
1313	extra_ies, extra_ies_len);
1314	break;
1315	case WLAN_TDLS_DISCOVERY_REQUEST:
1316	/*
1317	* Protect the discovery so we can hear the TDLS discovery
1318	* response frame. It is transmitted directly and not buffered
1319	* by the AP.
1320	*/
1321	drv_mgd_protect_tdls_discover(local: sdata->local, sdata, link_id);
1322	fallthrough;
1323	case WLAN_TDLS_SETUP_CONFIRM:
1324	case WLAN_PUB_ACTION_TDLS_DISCOVER_RES:
1325	/* no special handling */
1326	ret = ieee80211_tdls_prep_mgmt_packet(wiphy, dev, peer,
1327	link_id, action_code,
1328	dialog_token,
1329	status_code,
1330	peer_capability,
1331	initiator, extra_ies,
1332	extra_ies_len, oper_class: 0, NULL);
1333	break;
1334	default:
1335	ret = -EOPNOTSUPP;
1336	break;
1337	}
1338
1339	tdls_dbg(sdata, "TDLS mgmt action %d peer %pM link_id %d status %d\n",
1340	action_code, peer, link_id, ret);
1341	return ret;
1342	}
1343
1344	static void iee80211_tdls_recalc_chanctx(struct ieee80211_sub_if_data *sdata,
1345	struct sta_info *sta)
1346	{
1347	struct ieee80211_local *local = sdata->local;
1348	struct ieee80211_chanctx_conf *conf;
1349	struct ieee80211_chanctx *ctx;
1350	enum nl80211_chan_width width;
1351	struct ieee80211_supported_band *sband;
1352
1353	lockdep_assert_wiphy(local->hw.wiphy);
1354
1355	conf = rcu_dereference_protected(sdata->vif.bss_conf.chanctx_conf,
1356	lockdep_is_held(&local->hw.wiphy->mtx));
1357	if (conf) {
1358	width = conf->def.width;
1359	sband = local->hw.wiphy->bands[conf->def.chan->band];
1360	ctx = container_of(conf, struct ieee80211_chanctx, conf);
1361	ieee80211_recalc_chanctx_chantype(local, ctx);
1362
1363	/* if width changed and a peer is given, update its BW */
1364	if (width != conf->def.width && sta &&
1365	test_sta_flag(sta, flag: WLAN_STA_TDLS_WIDER_BW)) {
1366	enum ieee80211_sta_rx_bandwidth bw;
1367
1368	bw = ieee80211_chan_width_to_rx_bw(width: conf->def.width);
1369	bw = min(bw, ieee80211_sta_cap_rx_bw(&sta->deflink));
1370	if (bw != sta->sta.deflink.bandwidth) {
1371	sta->sta.deflink.bandwidth = bw;
1372	rate_control_rate_update(local, sband, sta, link_id: 0,
1373	changed: IEEE80211_RC_BW_CHANGED);
1374	/*
1375	* if a TDLS peer BW was updated, we need to
1376	* recalc the chandef width again, to get the
1377	* correct chanctx min_def
1378	*/
1379	ieee80211_recalc_chanctx_chantype(local, ctx);
1380	}
1381	}
1382
1383	}
1384	}
1385
1386	static int iee80211_tdls_have_ht_peers(struct ieee80211_sub_if_data *sdata)
1387	{
1388	struct sta_info *sta;
1389	bool result = false;
1390
1391	rcu_read_lock();
1392	list_for_each_entry_rcu(sta, &sdata->local->sta_list, list) {
1393	if (!sta->sta.tdls || sta->sdata != sdata || !sta->uploaded ||
1394	!test_sta_flag(sta, flag: WLAN_STA_AUTHORIZED) ||
1395	!test_sta_flag(sta, flag: WLAN_STA_TDLS_PEER_AUTH) ||
1396	!sta->sta.deflink.ht_cap.ht_supported)
1397	continue;
1398	result = true;
1399	break;
1400	}
1401	rcu_read_unlock();
1402
1403	return result;
1404	}
1405
1406	static void
1407	iee80211_tdls_recalc_ht_protection(struct ieee80211_sub_if_data *sdata,
1408	struct sta_info *sta)
1409	{
1410	bool tdls_ht;
1411	u16 protection = IEEE80211_HT_OP_MODE_PROTECTION_NONHT_MIXED |
1412	IEEE80211_HT_OP_MODE_NON_GF_STA_PRSNT |
1413	IEEE80211_HT_OP_MODE_NON_HT_STA_PRSNT;
1414	u16 opmode;
1415
1416	/* Nothing to do if the BSS connection uses HT */
1417	if (!(sdata->deflink.u.mgd.conn_flags & IEEE80211_CONN_DISABLE_HT))
1418	return;
1419
1420	tdls_ht = (sta && sta->sta.deflink.ht_cap.ht_supported) ||
1421	iee80211_tdls_have_ht_peers(sdata);
1422
1423	opmode = sdata->vif.bss_conf.ht_operation_mode;
1424
1425	if (tdls_ht)
1426	opmode |= protection;
1427	else
1428	opmode &= ~protection;
1429
1430	if (opmode == sdata->vif.bss_conf.ht_operation_mode)
1431	return;
1432
1433	sdata->vif.bss_conf.ht_operation_mode = opmode;
1434	ieee80211_link_info_change_notify(sdata, link: &sdata->deflink,
1435	changed: BSS_CHANGED_HT);
1436	}
1437
1438	int ieee80211_tdls_oper(struct wiphy *wiphy, struct net_device *dev,
1439	const u8 *peer, enum nl80211_tdls_operation oper)
1440	{
1441	struct sta_info *sta;
1442	struct ieee80211_sub_if_data *sdata = IEEE80211_DEV_TO_SUB_IF(dev);
1443	struct ieee80211_local *local = sdata->local;
1444	int ret;
1445
1446	lockdep_assert_wiphy(local->hw.wiphy);
1447
1448	if (!(wiphy->flags & WIPHY_FLAG_SUPPORTS_TDLS))
1449	return -ENOTSUPP;
1450
1451	if (sdata->vif.type != NL80211_IFTYPE_STATION)
1452	return -EINVAL;
1453
1454	switch (oper) {
1455	case NL80211_TDLS_ENABLE_LINK:
1456	case NL80211_TDLS_DISABLE_LINK:
1457	break;
1458	case NL80211_TDLS_TEARDOWN:
1459	case NL80211_TDLS_SETUP:
1460	case NL80211_TDLS_DISCOVERY_REQ:
1461	/* We don't support in-driver setup/teardown/discovery */
1462	return -ENOTSUPP;
1463	}
1464
1465	/* protect possible bss_conf changes and avoid concurrency in
1466	* ieee80211_bss_info_change_notify()
1467	*/
1468	tdls_dbg(sdata, "TDLS oper %d peer %pM\n", oper, peer);
1469
1470	switch (oper) {
1471	case NL80211_TDLS_ENABLE_LINK:
1472	if (sdata->vif.bss_conf.csa_active) {
1473	tdls_dbg(sdata, "TDLS: disallow link during CSA\n");
1474	return -EBUSY;
1475	}
1476
1477	sta = sta_info_get(sdata, addr: peer);
1478	if (!sta)
1479	return -ENOLINK;
1480
1481	iee80211_tdls_recalc_chanctx(sdata, sta);
1482	iee80211_tdls_recalc_ht_protection(sdata, sta);
1483
1484	set_sta_flag(sta, flag: WLAN_STA_TDLS_PEER_AUTH);
1485
1486	WARN_ON_ONCE(is_zero_ether_addr(sdata->u.mgd.tdls_peer) ||
1487	!ether_addr_equal(sdata->u.mgd.tdls_peer, peer));
1488	break;
1489	case NL80211_TDLS_DISABLE_LINK:
1490	/*
1491	* The teardown message in ieee80211_tdls_mgmt_teardown() was
1492	* created while the queues were stopped, so it might still be
1493	* pending. Before flushing the queues we need to be sure the
1494	* message is handled by the tasklet handling pending messages,
1495	* otherwise we might start destroying the station before
1496	* sending the teardown packet.
1497	* Note that this only forces the tasklet to flush pendings -
1498	* not to stop the tasklet from rescheduling itself.
1499	*/
1500	tasklet_kill(t: &local->tx_pending_tasklet);
1501	/* flush a potentially queued teardown packet */
1502	ieee80211_flush_queues(local, sdata, drop: false);
1503
1504	ret = sta_info_destroy_addr(sdata, addr: peer);
1505
1506	iee80211_tdls_recalc_ht_protection(sdata, NULL);
1507
1508	iee80211_tdls_recalc_chanctx(sdata, NULL);
1509	if (ret)
1510	return ret;
1511	break;
1512	default:
1513	return -ENOTSUPP;
1514	}
1515
1516	if (ether_addr_equal(addr1: sdata->u.mgd.tdls_peer, addr2: peer)) {
1517	wiphy_delayed_work_cancel(wiphy: sdata->local->hw.wiphy,
1518	dwork: &sdata->u.mgd.tdls_peer_del_work);
1519	eth_zero_addr(addr: sdata->u.mgd.tdls_peer);
1520	}
1521
1522	wiphy_work_queue(wiphy: sdata->local->hw.wiphy,
1523	work: &sdata->deflink.u.mgd.request_smps_work);
1524
1525	return 0;
1526	}
1527
1528	void ieee80211_tdls_oper_request(struct ieee80211_vif *vif, const u8 *peer,
1529	enum nl80211_tdls_operation oper,
1530	u16 reason_code, gfp_t gfp)
1531	{
1532	struct ieee80211_sub_if_data *sdata = vif_to_sdata(p: vif);
1533
1534	if (vif->type != NL80211_IFTYPE_STATION || !vif->cfg.assoc) {
1535	sdata_err(sdata, "Discarding TDLS oper %d - not STA or disconnected\n",
1536	oper);
1537	return;
1538	}
1539
1540	cfg80211_tdls_oper_request(dev: sdata->dev, peer, oper, reason_code, gfp);
1541	}
1542	EXPORT_SYMBOL(ieee80211_tdls_oper_request);
1543
1544	static void
1545	iee80211_tdls_add_ch_switch_timing(u8 *buf, u16 switch_time, u16 switch_timeout)
1546	{
1547	struct ieee80211_ch_switch_timing *ch_sw;
1548
1549	*buf++ = WLAN_EID_CHAN_SWITCH_TIMING;
1550	*buf++ = sizeof(struct ieee80211_ch_switch_timing);
1551
1552	ch_sw = (void *)buf;
1553	ch_sw->switch_time = cpu_to_le16(switch_time);
1554	ch_sw->switch_timeout = cpu_to_le16(switch_timeout);
1555	}
1556
1557	/* find switch timing IE in SKB ready for Tx */
1558	static const u8 *ieee80211_tdls_find_sw_timing_ie(struct sk_buff *skb)
1559	{
1560	struct ieee80211_tdls_data *tf;
1561	const u8 *ie_start;
1562
1563	/*
1564	* Get the offset for the new location of the switch timing IE.
1565	* The SKB network header will now point to the "payload_type"
1566	* element of the TDLS data frame struct.
1567	*/
1568	tf = container_of(skb->data + skb_network_offset(skb),
1569	struct ieee80211_tdls_data, payload_type);
1570	ie_start = tf->u.chan_switch_req.variable;
1571	return cfg80211_find_ie(eid: WLAN_EID_CHAN_SWITCH_TIMING, ies: ie_start,
1572	len: skb->len - (ie_start - skb->data));
1573	}
1574
1575	static struct sk_buff *
1576	ieee80211_tdls_ch_sw_tmpl_get(struct sta_info *sta, u8 oper_class,
1577	struct cfg80211_chan_def *chandef,
1578	u32 *ch_sw_tm_ie_offset)
1579	{
1580	struct ieee80211_sub_if_data *sdata = sta->sdata;
1581	u8 extra_ies[2 + sizeof(struct ieee80211_sec_chan_offs_ie) +
1582	2 + sizeof(struct ieee80211_ch_switch_timing)];
1583	int extra_ies_len = 2 + sizeof(struct ieee80211_ch_switch_timing);
1584	u8 *pos = extra_ies;
1585	struct sk_buff *skb;
1586	int link_id = sta->sta.valid_links ? ffs(sta->sta.valid_links) - 1 : 0;
1587
1588	/*
1589	* if chandef points to a wide channel add a Secondary-Channel
1590	* Offset information element
1591	*/
1592	if (chandef->width == NL80211_CHAN_WIDTH_40) {
1593	struct ieee80211_sec_chan_offs_ie *sec_chan_ie;
1594	bool ht40plus;
1595
1596	*pos++ = WLAN_EID_SECONDARY_CHANNEL_OFFSET;
1597	*pos++ = sizeof(*sec_chan_ie);
1598	sec_chan_ie = (void *)pos;
1599
1600	ht40plus = cfg80211_get_chandef_type(chandef) ==
1601	NL80211_CHAN_HT40PLUS;
1602	sec_chan_ie->sec_chan_offs = ht40plus ?
1603	IEEE80211_HT_PARAM_CHA_SEC_ABOVE :
1604	IEEE80211_HT_PARAM_CHA_SEC_BELOW;
1605	pos += sizeof(*sec_chan_ie);
1606
1607	extra_ies_len += 2 + sizeof(struct ieee80211_sec_chan_offs_ie);
1608	}
1609
1610	/* just set the values to 0, this is a template */
1611	iee80211_tdls_add_ch_switch_timing(buf: pos, switch_time: 0, switch_timeout: 0);
1612
1613	skb = ieee80211_tdls_build_mgmt_packet_data(sdata, peer: sta->sta.addr,
1614	link_id,
1615	action_code: WLAN_TDLS_CHANNEL_SWITCH_REQUEST,
1616	dialog_token: 0, status_code: 0, initiator: !sta->sta.tdls_initiator,
1617	extra_ies, extra_ies_len,
1618	oper_class, chandef);
1619	if (!skb)
1620	return NULL;
1621
1622	skb = ieee80211_build_data_template(sdata, skb, info_flags: 0);
1623	if (IS_ERR(ptr: skb)) {
1624	tdls_dbg(sdata, "Failed building TDLS channel switch frame\n");
1625	return NULL;
1626	}
1627
1628	if (ch_sw_tm_ie_offset) {
1629	const u8 *tm_ie = ieee80211_tdls_find_sw_timing_ie(skb);
1630
1631	if (!tm_ie) {
1632	tdls_dbg(sdata, "No switch timing IE in TDLS switch\n");
1633	dev_kfree_skb_any(skb);
1634	return NULL;
1635	}
1636
1637	*ch_sw_tm_ie_offset = tm_ie - skb->data;
1638	}
1639
1640	tdls_dbg(sdata,
1641	"TDLS channel switch request template for %pM ch %d width %d\n",
1642	sta->sta.addr, chandef->chan->center_freq, chandef->width);
1643	return skb;
1644	}
1645
1646	int
1647	ieee80211_tdls_channel_switch(struct wiphy *wiphy, struct net_device *dev,
1648	const u8 *addr, u8 oper_class,
1649	struct cfg80211_chan_def *chandef)
1650	{
1651	struct ieee80211_sub_if_data *sdata = IEEE80211_DEV_TO_SUB_IF(dev);
1652	struct ieee80211_local *local = sdata->local;
1653	struct sta_info *sta;
1654	struct sk_buff *skb = NULL;
1655	u32 ch_sw_tm_ie;
1656	int ret;
1657
1658	lockdep_assert_wiphy(local->hw.wiphy);
1659
1660	if (chandef->chan->freq_offset)
1661	/* this may work, but is untested */
1662	return -EOPNOTSUPP;
1663
1664	sta = sta_info_get(sdata, addr);
1665	if (!sta) {
1666	tdls_dbg(sdata,
1667	"Invalid TDLS peer %pM for channel switch request\n",
1668	addr);
1669	ret = -ENOENT;
1670	goto out;
1671	}
1672
1673	if (!test_sta_flag(sta, flag: WLAN_STA_TDLS_CHAN_SWITCH)) {
1674	tdls_dbg(sdata, "TDLS channel switch unsupported by %pM\n",
1675	addr);
1676	ret = -ENOTSUPP;
1677	goto out;
1678	}
1679
1680	skb = ieee80211_tdls_ch_sw_tmpl_get(sta, oper_class, chandef,
1681	ch_sw_tm_ie_offset: &ch_sw_tm_ie);
1682	if (!skb) {
1683	ret = -ENOENT;
1684	goto out;
1685	}
1686
1687	ret = drv_tdls_channel_switch(local, sdata, sta: &sta->sta, oper_class,
1688	chandef, tmpl_skb: skb, ch_sw_tm_ie);
1689	if (!ret)
1690	set_sta_flag(sta, flag: WLAN_STA_TDLS_OFF_CHANNEL);
1691
1692	out:
1693	dev_kfree_skb_any(skb);
1694	return ret;
1695	}
1696
1697	void
1698	ieee80211_tdls_cancel_channel_switch(struct wiphy *wiphy,
1699	struct net_device *dev,
1700	const u8 *addr)
1701	{
1702	struct ieee80211_sub_if_data *sdata = IEEE80211_DEV_TO_SUB_IF(dev);
1703	struct ieee80211_local *local = sdata->local;
1704	struct sta_info *sta;
1705
1706	lockdep_assert_wiphy(local->hw.wiphy);
1707
1708	sta = sta_info_get(sdata, addr);
1709	if (!sta) {
1710	tdls_dbg(sdata,
1711	"Invalid TDLS peer %pM for channel switch cancel\n",
1712	addr);
1713	return;
1714	}
1715
1716	if (!test_sta_flag(sta, flag: WLAN_STA_TDLS_OFF_CHANNEL)) {
1717	tdls_dbg(sdata, "TDLS channel switch not initiated by %pM\n",
1718	addr);
1719	return;
1720	}
1721
1722	drv_tdls_cancel_channel_switch(local, sdata, sta: &sta->sta);
1723	clear_sta_flag(sta, flag: WLAN_STA_TDLS_OFF_CHANNEL);
1724	}
1725
1726	static struct sk_buff *
1727	ieee80211_tdls_ch_sw_resp_tmpl_get(struct sta_info *sta,
1728	u32 *ch_sw_tm_ie_offset)
1729	{
1730	struct ieee80211_sub_if_data *sdata = sta->sdata;
1731	struct sk_buff *skb;
1732	u8 extra_ies[2 + sizeof(struct ieee80211_ch_switch_timing)];
1733	int link_id = sta->sta.valid_links ? ffs(sta->sta.valid_links) - 1 : 0;
1734
1735	/* initial timing are always zero in the template */
1736	iee80211_tdls_add_ch_switch_timing(buf: extra_ies, switch_time: 0, switch_timeout: 0);
1737
1738	skb = ieee80211_tdls_build_mgmt_packet_data(sdata, peer: sta->sta.addr,
1739	link_id,
1740	action_code: WLAN_TDLS_CHANNEL_SWITCH_RESPONSE,
1741	dialog_token: 0, status_code: 0, initiator: !sta->sta.tdls_initiator,
1742	extra_ies, extra_ies_len: sizeof(extra_ies), oper_class: 0, NULL);
1743	if (!skb)
1744	return NULL;
1745
1746	skb = ieee80211_build_data_template(sdata, skb, info_flags: 0);
1747	if (IS_ERR(ptr: skb)) {
1748	tdls_dbg(sdata,
1749	"Failed building TDLS channel switch resp frame\n");
1750	return NULL;
1751	}
1752
1753	if (ch_sw_tm_ie_offset) {
1754	const u8 *tm_ie = ieee80211_tdls_find_sw_timing_ie(skb);
1755
1756	if (!tm_ie) {
1757	tdls_dbg(sdata,
1758	"No switch timing IE in TDLS switch resp\n");
1759	dev_kfree_skb_any(skb);
1760	return NULL;
1761	}
1762
1763	*ch_sw_tm_ie_offset = tm_ie - skb->data;
1764	}
1765
1766	tdls_dbg(sdata, "TDLS get channel switch response template for %pM\n",
1767	sta->sta.addr);
1768	return skb;
1769	}
1770
1771	static int
1772	ieee80211_process_tdls_channel_switch_resp(struct ieee80211_sub_if_data *sdata,
1773	struct sk_buff *skb)
1774	{
1775	struct ieee80211_local *local = sdata->local;
1776	struct ieee802_11_elems *elems = NULL;
1777	struct sta_info *sta;
1778	struct ieee80211_tdls_data *tf = (void *)skb->data;
1779	bool local_initiator;
1780	struct ieee80211_rx_status *rx_status = IEEE80211_SKB_RXCB(skb);
1781	int baselen = offsetof(typeof(*tf), u.chan_switch_resp.variable);
1782	struct ieee80211_tdls_ch_sw_params params = {};
1783	int ret;
1784
1785	lockdep_assert_wiphy(local->hw.wiphy);
1786
1787	params.action_code = WLAN_TDLS_CHANNEL_SWITCH_RESPONSE;
1788	params.timestamp = rx_status->device_timestamp;
1789
1790	if (skb->len < baselen) {
1791	tdls_dbg(sdata, "TDLS channel switch resp too short: %d\n",
1792	skb->len);
1793	return -EINVAL;
1794	}
1795
1796	sta = sta_info_get(sdata, addr: tf->sa);
1797	if (!sta || !test_sta_flag(sta, flag: WLAN_STA_TDLS_PEER_AUTH)) {
1798	tdls_dbg(sdata, "TDLS chan switch from non-peer sta %pM\n",
1799	tf->sa);
1800	ret = -EINVAL;
1801	goto out;
1802	}
1803
1804	params.sta = &sta->sta;
1805	params.status = le16_to_cpu(tf->u.chan_switch_resp.status_code);
1806	if (params.status != 0) {
1807	ret = 0;
1808	goto call_drv;
1809	}
1810
1811	elems = ieee802_11_parse_elems(start: tf->u.chan_switch_resp.variable,
1812	len: skb->len - baselen, action: false, NULL);
1813	if (!elems) {
1814	ret = -ENOMEM;
1815	goto out;
1816	}
1817
1818	if (elems->parse_error) {
1819	tdls_dbg(sdata, "Invalid IEs in TDLS channel switch resp\n");
1820	ret = -EINVAL;
1821	goto out;
1822	}
1823
1824	if (!elems->ch_sw_timing || !elems->lnk_id) {
1825	tdls_dbg(sdata, "TDLS channel switch resp - missing IEs\n");
1826	ret = -EINVAL;
1827	goto out;
1828	}
1829
1830	/* validate the initiator is set correctly */
1831	local_initiator =
1832	!memcmp(p: elems->lnk_id->init_sta, q: sdata->vif.addr, ETH_ALEN);
1833	if (local_initiator == sta->sta.tdls_initiator) {
1834	tdls_dbg(sdata, "TDLS chan switch invalid lnk-id initiator\n");
1835	ret = -EINVAL;
1836	goto out;
1837	}
1838
1839	params.switch_time = le16_to_cpu(elems->ch_sw_timing->switch_time);
1840	params.switch_timeout = le16_to_cpu(elems->ch_sw_timing->switch_timeout);
1841
1842	params.tmpl_skb =
1843	ieee80211_tdls_ch_sw_resp_tmpl_get(sta, ch_sw_tm_ie_offset: &params.ch_sw_tm_ie);
1844	if (!params.tmpl_skb) {
1845	ret = -ENOENT;
1846	goto out;
1847	}
1848
1849	ret = 0;
1850	call_drv:
1851	drv_tdls_recv_channel_switch(local: sdata->local, sdata, params: &params);
1852
1853	tdls_dbg(sdata,
1854	"TDLS channel switch response received from %pM status %d\n",
1855	tf->sa, params.status);
1856
1857	out:
1858	dev_kfree_skb_any(skb: params.tmpl_skb);
1859	kfree(objp: elems);
1860	return ret;
1861	}
1862
1863	static int
1864	ieee80211_process_tdls_channel_switch_req(struct ieee80211_sub_if_data *sdata,
1865	struct sk_buff *skb)
1866	{
1867	struct ieee80211_local *local = sdata->local;
1868	struct ieee802_11_elems *elems;
1869	struct cfg80211_chan_def chandef;
1870	struct ieee80211_channel *chan;
1871	enum nl80211_channel_type chan_type;
1872	int freq;
1873	u8 target_channel, oper_class;
1874	bool local_initiator;
1875	struct sta_info *sta;
1876	enum nl80211_band band;
1877	struct ieee80211_tdls_data *tf = (void *)skb->data;
1878	struct ieee80211_rx_status *rx_status = IEEE80211_SKB_RXCB(skb);
1879	int baselen = offsetof(typeof(*tf), u.chan_switch_req.variable);
1880	struct ieee80211_tdls_ch_sw_params params = {};
1881	int ret = 0;
1882
1883	lockdep_assert_wiphy(local->hw.wiphy);
1884
1885	params.action_code = WLAN_TDLS_CHANNEL_SWITCH_REQUEST;
1886	params.timestamp = rx_status->device_timestamp;
1887
1888	if (skb->len < baselen) {
1889	tdls_dbg(sdata, "TDLS channel switch req too short: %d\n",
1890	skb->len);
1891	return -EINVAL;
1892	}
1893
1894	target_channel = tf->u.chan_switch_req.target_channel;
1895	oper_class = tf->u.chan_switch_req.oper_class;
1896
1897	/*
1898	* We can't easily infer the channel band. The operating class is
1899	* ambiguous - there are multiple tables (US/Europe/JP/Global). The
1900	* solution here is to treat channels with number >14 as 5GHz ones,
1901	* and specifically check for the (oper_class, channel) combinations
1902	* where this doesn't hold. These are thankfully unique according to
1903	* IEEE802.11-2012.
1904	* We consider only the 2GHz and 5GHz bands and 20MHz+ channels as
1905	* valid here.
1906	*/
1907	if ((oper_class == 112 || oper_class == 2 || oper_class == 3 ||
1908	oper_class == 4 || oper_class == 5 || oper_class == 6) &&
1909	target_channel < 14)
1910	band = NL80211_BAND_5GHZ;
1911	else
1912	band = target_channel < 14 ? NL80211_BAND_2GHZ :
1913	NL80211_BAND_5GHZ;
1914
1915	freq = ieee80211_channel_to_frequency(chan: target_channel, band);
1916	if (freq == 0) {
1917	tdls_dbg(sdata, "Invalid channel in TDLS chan switch: %d\n",
1918	target_channel);
1919	return -EINVAL;
1920	}
1921
1922	chan = ieee80211_get_channel(wiphy: sdata->local->hw.wiphy, freq);
1923	if (!chan) {
1924	tdls_dbg(sdata,
1925	"Unsupported channel for TDLS chan switch: %d\n",
1926	target_channel);
1927	return -EINVAL;
1928	}
1929
1930	elems = ieee802_11_parse_elems(start: tf->u.chan_switch_req.variable,
1931	len: skb->len - baselen, action: false, NULL);
1932	if (!elems)
1933	return -ENOMEM;
1934
1935	if (elems->parse_error) {
1936	tdls_dbg(sdata, "Invalid IEs in TDLS channel switch req\n");
1937	ret = -EINVAL;
1938	goto free;
1939	}
1940
1941	if (!elems->ch_sw_timing || !elems->lnk_id) {
1942	tdls_dbg(sdata, "TDLS channel switch req - missing IEs\n");
1943	ret = -EINVAL;
1944	goto free;
1945	}
1946
1947	if (!elems->sec_chan_offs) {
1948	chan_type = NL80211_CHAN_HT20;
1949	} else {
1950	switch (elems->sec_chan_offs->sec_chan_offs) {
1951	case IEEE80211_HT_PARAM_CHA_SEC_ABOVE:
1952	chan_type = NL80211_CHAN_HT40PLUS;
1953	break;
1954	case IEEE80211_HT_PARAM_CHA_SEC_BELOW:
1955	chan_type = NL80211_CHAN_HT40MINUS;
1956	break;
1957	default:
1958	chan_type = NL80211_CHAN_HT20;
1959	break;
1960	}
1961	}
1962
1963	cfg80211_chandef_create(chandef: &chandef, channel: chan, chantype: chan_type);
1964
1965	/* we will be active on the TDLS link */
1966	if (!cfg80211_reg_can_beacon_relax(wiphy: sdata->local->hw.wiphy, chandef: &chandef,
1967	iftype: sdata->wdev.iftype)) {
1968	tdls_dbg(sdata, "TDLS chan switch to forbidden channel\n");
1969	ret = -EINVAL;
1970	goto free;
1971	}
1972
1973	sta = sta_info_get(sdata, addr: tf->sa);
1974	if (!sta || !test_sta_flag(sta, flag: WLAN_STA_TDLS_PEER_AUTH)) {
1975	tdls_dbg(sdata, "TDLS chan switch from non-peer sta %pM\n",
1976	tf->sa);
1977	ret = -EINVAL;
1978	goto out;
1979	}
1980
1981	params.sta = &sta->sta;
1982
1983	/* validate the initiator is set correctly */
1984	local_initiator =
1985	!memcmp(p: elems->lnk_id->init_sta, q: sdata->vif.addr, ETH_ALEN);
1986	if (local_initiator == sta->sta.tdls_initiator) {
1987	tdls_dbg(sdata, "TDLS chan switch invalid lnk-id initiator\n");
1988	ret = -EINVAL;
1989	goto out;
1990	}
1991
1992	/* peer should have known better */
1993	if (!sta->sta.deflink.ht_cap.ht_supported && elems->sec_chan_offs &&
1994	elems->sec_chan_offs->sec_chan_offs) {
1995	tdls_dbg(sdata, "TDLS chan switch - wide chan unsupported\n");
1996	ret = -ENOTSUPP;
1997	goto out;
1998	}
1999
2000	params.chandef = &chandef;
2001	params.switch_time = le16_to_cpu(elems->ch_sw_timing->switch_time);
2002	params.switch_timeout = le16_to_cpu(elems->ch_sw_timing->switch_timeout);
2003
2004	params.tmpl_skb =
2005	ieee80211_tdls_ch_sw_resp_tmpl_get(sta,
2006	ch_sw_tm_ie_offset: &params.ch_sw_tm_ie);
2007	if (!params.tmpl_skb) {
2008	ret = -ENOENT;
2009	goto out;
2010	}
2011
2012	drv_tdls_recv_channel_switch(local: sdata->local, sdata, params: &params);
2013
2014	tdls_dbg(sdata,
2015	"TDLS ch switch request received from %pM ch %d width %d\n",
2016	tf->sa, params.chandef->chan->center_freq,
2017	params.chandef->width);
2018	out:
2019	dev_kfree_skb_any(skb: params.tmpl_skb);
2020	free:
2021	kfree(objp: elems);
2022	return ret;
2023	}
2024
2025	void
2026	ieee80211_process_tdls_channel_switch(struct ieee80211_sub_if_data *sdata,
2027	struct sk_buff *skb)
2028	{
2029	struct ieee80211_tdls_data *tf = (void *)skb->data;
2030	struct wiphy *wiphy = sdata->local->hw.wiphy;
2031
2032	lockdep_assert_wiphy(wiphy);
2033
2034	/* make sure the driver supports it */
2035	if (!(wiphy->features & NL80211_FEATURE_TDLS_CHANNEL_SWITCH))
2036	return;
2037
2038	/* we want to access the entire packet */
2039	if (skb_linearize(skb))
2040	return;
2041	/*
2042	* The packet/size was already validated by mac80211 Rx path, only look
2043	* at the action type.
2044	*/
2045	switch (tf->action_code) {
2046	case WLAN_TDLS_CHANNEL_SWITCH_REQUEST:
2047	ieee80211_process_tdls_channel_switch_req(sdata, skb);
2048	break;
2049	case WLAN_TDLS_CHANNEL_SWITCH_RESPONSE:
2050	ieee80211_process_tdls_channel_switch_resp(sdata, skb);
2051	break;
2052	default:
2053	WARN_ON_ONCE(1);
2054	return;
2055	}
2056	}
2057
2058	void ieee80211_teardown_tdls_peers(struct ieee80211_sub_if_data *sdata)
2059	{
2060	struct sta_info *sta;
2061	u16 reason = WLAN_REASON_TDLS_TEARDOWN_UNSPECIFIED;
2062
2063	rcu_read_lock();
2064	list_for_each_entry_rcu(sta, &sdata->local->sta_list, list) {
2065	if (!sta->sta.tdls || sta->sdata != sdata || !sta->uploaded ||
2066	!test_sta_flag(sta, flag: WLAN_STA_AUTHORIZED))
2067	continue;
2068
2069	ieee80211_tdls_oper_request(&sdata->vif, sta->sta.addr,
2070	NL80211_TDLS_TEARDOWN, reason,
2071	GFP_ATOMIC);
2072	}
2073	rcu_read_unlock();
2074	}
2075
2076	void ieee80211_tdls_handle_disconnect(struct ieee80211_sub_if_data *sdata,
2077	const u8 *peer, u16 reason)
2078	{
2079	struct ieee80211_sta *sta;
2080
2081	rcu_read_lock();
2082	sta = ieee80211_find_sta(vif: &sdata->vif, addr: peer);
2083	if (!sta || !sta->tdls) {
2084	rcu_read_unlock();
2085	return;
2086	}
2087	rcu_read_unlock();
2088
2089	tdls_dbg(sdata, "disconnected from TDLS peer %pM (Reason: %u=%s)\n",
2090	peer, reason,
2091	ieee80211_get_reason_code_string(reason));
2092
2093	ieee80211_tdls_oper_request(&sdata->vif, peer,
2094	NL80211_TDLS_TEARDOWN,
2095	WLAN_REASON_TDLS_TEARDOWN_UNREACHABLE,
2096	GFP_ATOMIC);
2097	}
2098