xref: /openbmc/linux/net/mac80211/tx.c (revision d54455a3)
1 // SPDX-License-Identifier: GPL-2.0-only
2 /*
3  * Copyright 2002-2005, Instant802 Networks, Inc.
4  * Copyright 2005-2006, Devicescape Software, Inc.
5  * Copyright 2006-2007	Jiri Benc <jbenc@suse.cz>
6  * Copyright 2007	Johannes Berg <johannes@sipsolutions.net>
7  * Copyright 2013-2014  Intel Mobile Communications GmbH
8  * Copyright (C) 2018-2024 Intel Corporation
9  *
10  * Transmit and frame generation functions.
11  */
12 
13 #include <linux/kernel.h>
14 #include <linux/slab.h>
15 #include <linux/skbuff.h>
16 #include <linux/if_vlan.h>
17 #include <linux/etherdevice.h>
18 #include <linux/bitmap.h>
19 #include <linux/rcupdate.h>
20 #include <linux/export.h>
21 #include <net/net_namespace.h>
22 #include <net/ieee80211_radiotap.h>
23 #include <net/cfg80211.h>
24 #include <net/mac80211.h>
25 #include <net/codel.h>
26 #include <net/codel_impl.h>
27 #include <asm/unaligned.h>
28 #include <net/fq_impl.h>
29 #include <net/gso.h>
30 
31 #include "ieee80211_i.h"
32 #include "driver-ops.h"
33 #include "led.h"
34 #include "mesh.h"
35 #include "wep.h"
36 #include "wpa.h"
37 #include "wme.h"
38 #include "rate.h"
39 
40 /* misc utils */
41 
ieee80211_duration(struct ieee80211_tx_data * tx,struct sk_buff * skb,int group_addr,int next_frag_len)42 static __le16 ieee80211_duration(struct ieee80211_tx_data *tx,
43 				 struct sk_buff *skb, int group_addr,
44 				 int next_frag_len)
45 {
46 	int rate, mrate, erp, dur, i, shift = 0;
47 	struct ieee80211_rate *txrate;
48 	struct ieee80211_local *local = tx->local;
49 	struct ieee80211_supported_band *sband;
50 	struct ieee80211_hdr *hdr;
51 	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
52 	struct ieee80211_chanctx_conf *chanctx_conf;
53 	u32 rate_flags = 0;
54 
55 	/* assume HW handles this */
56 	if (tx->rate.flags & (IEEE80211_TX_RC_MCS | IEEE80211_TX_RC_VHT_MCS))
57 		return 0;
58 
59 	rcu_read_lock();
60 	chanctx_conf = rcu_dereference(tx->sdata->vif.bss_conf.chanctx_conf);
61 	if (chanctx_conf) {
62 		shift = ieee80211_chandef_get_shift(&chanctx_conf->def);
63 		rate_flags = ieee80211_chandef_rate_flags(&chanctx_conf->def);
64 	}
65 	rcu_read_unlock();
66 
67 	/* uh huh? */
68 	if (WARN_ON_ONCE(tx->rate.idx < 0))
69 		return 0;
70 
71 	sband = local->hw.wiphy->bands[info->band];
72 	txrate = &sband->bitrates[tx->rate.idx];
73 
74 	erp = txrate->flags & IEEE80211_RATE_ERP_G;
75 
76 	/* device is expected to do this */
77 	if (sband->band == NL80211_BAND_S1GHZ)
78 		return 0;
79 
80 	/*
81 	 * data and mgmt (except PS Poll):
82 	 * - during CFP: 32768
83 	 * - during contention period:
84 	 *   if addr1 is group address: 0
85 	 *   if more fragments = 0 and addr1 is individual address: time to
86 	 *      transmit one ACK plus SIFS
87 	 *   if more fragments = 1 and addr1 is individual address: time to
88 	 *      transmit next fragment plus 2 x ACK plus 3 x SIFS
89 	 *
90 	 * IEEE 802.11, 9.6:
91 	 * - control response frame (CTS or ACK) shall be transmitted using the
92 	 *   same rate as the immediately previous frame in the frame exchange
93 	 *   sequence, if this rate belongs to the PHY mandatory rates, or else
94 	 *   at the highest possible rate belonging to the PHY rates in the
95 	 *   BSSBasicRateSet
96 	 */
97 	hdr = (struct ieee80211_hdr *)skb->data;
98 	if (ieee80211_is_ctl(hdr->frame_control)) {
99 		/* TODO: These control frames are not currently sent by
100 		 * mac80211, but should they be implemented, this function
101 		 * needs to be updated to support duration field calculation.
102 		 *
103 		 * RTS: time needed to transmit pending data/mgmt frame plus
104 		 *    one CTS frame plus one ACK frame plus 3 x SIFS
105 		 * CTS: duration of immediately previous RTS minus time
106 		 *    required to transmit CTS and its SIFS
107 		 * ACK: 0 if immediately previous directed data/mgmt had
108 		 *    more=0, with more=1 duration in ACK frame is duration
109 		 *    from previous frame minus time needed to transmit ACK
110 		 *    and its SIFS
111 		 * PS Poll: BIT(15) | BIT(14) | aid
112 		 */
113 		return 0;
114 	}
115 
116 	/* data/mgmt */
117 	if (0 /* FIX: data/mgmt during CFP */)
118 		return cpu_to_le16(32768);
119 
120 	if (group_addr) /* Group address as the destination - no ACK */
121 		return 0;
122 
123 	/* Individual destination address:
124 	 * IEEE 802.11, Ch. 9.6 (after IEEE 802.11g changes)
125 	 * CTS and ACK frames shall be transmitted using the highest rate in
126 	 * basic rate set that is less than or equal to the rate of the
127 	 * immediately previous frame and that is using the same modulation
128 	 * (CCK or OFDM). If no basic rate set matches with these requirements,
129 	 * the highest mandatory rate of the PHY that is less than or equal to
130 	 * the rate of the previous frame is used.
131 	 * Mandatory rates for IEEE 802.11g PHY: 1, 2, 5.5, 11, 6, 12, 24 Mbps
132 	 */
133 	rate = -1;
134 	/* use lowest available if everything fails */
135 	mrate = sband->bitrates[0].bitrate;
136 	for (i = 0; i < sband->n_bitrates; i++) {
137 		struct ieee80211_rate *r = &sband->bitrates[i];
138 
139 		if (r->bitrate > txrate->bitrate)
140 			break;
141 
142 		if ((rate_flags & r->flags) != rate_flags)
143 			continue;
144 
145 		if (tx->sdata->vif.bss_conf.basic_rates & BIT(i))
146 			rate = DIV_ROUND_UP(r->bitrate, 1 << shift);
147 
148 		switch (sband->band) {
149 		case NL80211_BAND_2GHZ:
150 		case NL80211_BAND_LC: {
151 			u32 flag;
152 			if (tx->sdata->deflink.operating_11g_mode)
153 				flag = IEEE80211_RATE_MANDATORY_G;
154 			else
155 				flag = IEEE80211_RATE_MANDATORY_B;
156 			if (r->flags & flag)
157 				mrate = r->bitrate;
158 			break;
159 		}
160 		case NL80211_BAND_5GHZ:
161 		case NL80211_BAND_6GHZ:
162 			if (r->flags & IEEE80211_RATE_MANDATORY_A)
163 				mrate = r->bitrate;
164 			break;
165 		case NL80211_BAND_S1GHZ:
166 		case NL80211_BAND_60GHZ:
167 			/* TODO, for now fall through */
168 		case NUM_NL80211_BANDS:
169 			WARN_ON(1);
170 			break;
171 		}
172 	}
173 	if (rate == -1) {
174 		/* No matching basic rate found; use highest suitable mandatory
175 		 * PHY rate */
176 		rate = DIV_ROUND_UP(mrate, 1 << shift);
177 	}
178 
179 	/* Don't calculate ACKs for QoS Frames with NoAck Policy set */
180 	if (ieee80211_is_data_qos(hdr->frame_control) &&
181 	    *(ieee80211_get_qos_ctl(hdr)) & IEEE80211_QOS_CTL_ACK_POLICY_NOACK)
182 		dur = 0;
183 	else
184 		/* Time needed to transmit ACK
185 		 * (10 bytes + 4-byte FCS = 112 bits) plus SIFS; rounded up
186 		 * to closest integer */
187 		dur = ieee80211_frame_duration(sband->band, 10, rate, erp,
188 				tx->sdata->vif.bss_conf.use_short_preamble,
189 				shift);
190 
191 	if (next_frag_len) {
192 		/* Frame is fragmented: duration increases with time needed to
193 		 * transmit next fragment plus ACK and 2 x SIFS. */
194 		dur *= 2; /* ACK + SIFS */
195 		/* next fragment */
196 		dur += ieee80211_frame_duration(sband->band, next_frag_len,
197 				txrate->bitrate, erp,
198 				tx->sdata->vif.bss_conf.use_short_preamble,
199 				shift);
200 	}
201 
202 	return cpu_to_le16(dur);
203 }
204 
205 /* tx handlers */
206 static ieee80211_tx_result debug_noinline
ieee80211_tx_h_dynamic_ps(struct ieee80211_tx_data * tx)207 ieee80211_tx_h_dynamic_ps(struct ieee80211_tx_data *tx)
208 {
209 	struct ieee80211_local *local = tx->local;
210 	struct ieee80211_if_managed *ifmgd;
211 	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(tx->skb);
212 
213 	/* driver doesn't support power save */
214 	if (!ieee80211_hw_check(&local->hw, SUPPORTS_PS))
215 		return TX_CONTINUE;
216 
217 	/* hardware does dynamic power save */
218 	if (ieee80211_hw_check(&local->hw, SUPPORTS_DYNAMIC_PS))
219 		return TX_CONTINUE;
220 
221 	/* dynamic power save disabled */
222 	if (local->hw.conf.dynamic_ps_timeout <= 0)
223 		return TX_CONTINUE;
224 
225 	/* we are scanning, don't enable power save */
226 	if (local->scanning)
227 		return TX_CONTINUE;
228 
229 	if (!local->ps_sdata)
230 		return TX_CONTINUE;
231 
232 	/* No point if we're going to suspend */
233 	if (local->quiescing)
234 		return TX_CONTINUE;
235 
236 	/* dynamic ps is supported only in managed mode */
237 	if (tx->sdata->vif.type != NL80211_IFTYPE_STATION)
238 		return TX_CONTINUE;
239 
240 	if (unlikely(info->flags & IEEE80211_TX_INTFL_OFFCHAN_TX_OK))
241 		return TX_CONTINUE;
242 
243 	ifmgd = &tx->sdata->u.mgd;
244 
245 	/*
246 	 * Don't wakeup from power save if u-apsd is enabled, voip ac has
247 	 * u-apsd enabled and the frame is in voip class. This effectively
248 	 * means that even if all access categories have u-apsd enabled, in
249 	 * practise u-apsd is only used with the voip ac. This is a
250 	 * workaround for the case when received voip class packets do not
251 	 * have correct qos tag for some reason, due the network or the
252 	 * peer application.
253 	 *
254 	 * Note: ifmgd->uapsd_queues access is racy here. If the value is
255 	 * changed via debugfs, user needs to reassociate manually to have
256 	 * everything in sync.
257 	 */
258 	if ((ifmgd->flags & IEEE80211_STA_UAPSD_ENABLED) &&
259 	    (ifmgd->uapsd_queues & IEEE80211_WMM_IE_STA_QOSINFO_AC_VO) &&
260 	    skb_get_queue_mapping(tx->skb) == IEEE80211_AC_VO)
261 		return TX_CONTINUE;
262 
263 	if (local->hw.conf.flags & IEEE80211_CONF_PS) {
264 		ieee80211_stop_queues_by_reason(&local->hw,
265 						IEEE80211_MAX_QUEUE_MAP,
266 						IEEE80211_QUEUE_STOP_REASON_PS,
267 						false);
268 		ifmgd->flags &= ~IEEE80211_STA_NULLFUNC_ACKED;
269 		ieee80211_queue_work(&local->hw,
270 				     &local->dynamic_ps_disable_work);
271 	}
272 
273 	/* Don't restart the timer if we're not disassociated */
274 	if (!ifmgd->associated)
275 		return TX_CONTINUE;
276 
277 	mod_timer(&local->dynamic_ps_timer, jiffies +
278 		  msecs_to_jiffies(local->hw.conf.dynamic_ps_timeout));
279 
280 	return TX_CONTINUE;
281 }
282 
283 static ieee80211_tx_result debug_noinline
ieee80211_tx_h_check_assoc(struct ieee80211_tx_data * tx)284 ieee80211_tx_h_check_assoc(struct ieee80211_tx_data *tx)
285 {
286 
287 	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)tx->skb->data;
288 	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(tx->skb);
289 	bool assoc = false;
290 
291 	if (unlikely(info->flags & IEEE80211_TX_CTL_INJECTED))
292 		return TX_CONTINUE;
293 
294 	if (unlikely(test_bit(SCAN_SW_SCANNING, &tx->local->scanning)) &&
295 	    test_bit(SDATA_STATE_OFFCHANNEL, &tx->sdata->state) &&
296 	    !ieee80211_is_probe_req(hdr->frame_control) &&
297 	    !ieee80211_is_any_nullfunc(hdr->frame_control))
298 		/*
299 		 * When software scanning only nullfunc frames (to notify
300 		 * the sleep state to the AP) and probe requests (for the
301 		 * active scan) are allowed, all other frames should not be
302 		 * sent and we should not get here, but if we do
303 		 * nonetheless, drop them to avoid sending them
304 		 * off-channel. See the link below and
305 		 * ieee80211_start_scan() for more.
306 		 *
307 		 * http://article.gmane.org/gmane.linux.kernel.wireless.general/30089
308 		 */
309 		return TX_DROP;
310 
311 	if (tx->sdata->vif.type == NL80211_IFTYPE_OCB)
312 		return TX_CONTINUE;
313 
314 	if (tx->flags & IEEE80211_TX_PS_BUFFERED)
315 		return TX_CONTINUE;
316 
317 	if (tx->sta)
318 		assoc = test_sta_flag(tx->sta, WLAN_STA_ASSOC);
319 
320 	if (likely(tx->flags & IEEE80211_TX_UNICAST)) {
321 		if (unlikely(!assoc &&
322 			     ieee80211_is_data(hdr->frame_control))) {
323 #ifdef CONFIG_MAC80211_VERBOSE_DEBUG
324 			sdata_info(tx->sdata,
325 				   "dropped data frame to not associated station %pM\n",
326 				   hdr->addr1);
327 #endif
328 			I802_DEBUG_INC(tx->local->tx_handlers_drop_not_assoc);
329 			return TX_DROP;
330 		}
331 	} else if (unlikely(ieee80211_is_data(hdr->frame_control) &&
332 			    ieee80211_vif_get_num_mcast_if(tx->sdata) == 0)) {
333 		/*
334 		 * No associated STAs - no need to send multicast
335 		 * frames.
336 		 */
337 		return TX_DROP;
338 	}
339 
340 	return TX_CONTINUE;
341 }
342 
343 /* This function is called whenever the AP is about to exceed the maximum limit
344  * of buffered frames for power saving STAs. This situation should not really
345  * happen often during normal operation, so dropping the oldest buffered packet
346  * from each queue should be OK to make some room for new frames. */
purge_old_ps_buffers(struct ieee80211_local * local)347 static void purge_old_ps_buffers(struct ieee80211_local *local)
348 {
349 	int total = 0, purged = 0;
350 	struct sk_buff *skb;
351 	struct ieee80211_sub_if_data *sdata;
352 	struct sta_info *sta;
353 
354 	list_for_each_entry_rcu(sdata, &local->interfaces, list) {
355 		struct ps_data *ps;
356 
357 		if (sdata->vif.type == NL80211_IFTYPE_AP)
358 			ps = &sdata->u.ap.ps;
359 		else if (ieee80211_vif_is_mesh(&sdata->vif))
360 			ps = &sdata->u.mesh.ps;
361 		else
362 			continue;
363 
364 		skb = skb_dequeue(&ps->bc_buf);
365 		if (skb) {
366 			purged++;
367 			ieee80211_free_txskb(&local->hw, skb);
368 		}
369 		total += skb_queue_len(&ps->bc_buf);
370 	}
371 
372 	/*
373 	 * Drop one frame from each station from the lowest-priority
374 	 * AC that has frames at all.
375 	 */
376 	list_for_each_entry_rcu(sta, &local->sta_list, list) {
377 		int ac;
378 
379 		for (ac = IEEE80211_AC_BK; ac >= IEEE80211_AC_VO; ac--) {
380 			skb = skb_dequeue(&sta->ps_tx_buf[ac]);
381 			total += skb_queue_len(&sta->ps_tx_buf[ac]);
382 			if (skb) {
383 				purged++;
384 				ieee80211_free_txskb(&local->hw, skb);
385 				break;
386 			}
387 		}
388 	}
389 
390 	local->total_ps_buffered = total;
391 	ps_dbg_hw(&local->hw, "PS buffers full - purged %d frames\n", purged);
392 }
393 
394 static ieee80211_tx_result
ieee80211_tx_h_multicast_ps_buf(struct ieee80211_tx_data * tx)395 ieee80211_tx_h_multicast_ps_buf(struct ieee80211_tx_data *tx)
396 {
397 	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(tx->skb);
398 	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)tx->skb->data;
399 	struct ps_data *ps;
400 
401 	/*
402 	 * broadcast/multicast frame
403 	 *
404 	 * If any of the associated/peer stations is in power save mode,
405 	 * the frame is buffered to be sent after DTIM beacon frame.
406 	 * This is done either by the hardware or us.
407 	 */
408 
409 	/* powersaving STAs currently only in AP/VLAN/mesh mode */
410 	if (tx->sdata->vif.type == NL80211_IFTYPE_AP ||
411 	    tx->sdata->vif.type == NL80211_IFTYPE_AP_VLAN) {
412 		if (!tx->sdata->bss)
413 			return TX_CONTINUE;
414 
415 		ps = &tx->sdata->bss->ps;
416 	} else if (ieee80211_vif_is_mesh(&tx->sdata->vif)) {
417 		ps = &tx->sdata->u.mesh.ps;
418 	} else {
419 		return TX_CONTINUE;
420 	}
421 
422 
423 	/* no buffering for ordered frames */
424 	if (ieee80211_has_order(hdr->frame_control))
425 		return TX_CONTINUE;
426 
427 	if (ieee80211_is_probe_req(hdr->frame_control))
428 		return TX_CONTINUE;
429 
430 	if (ieee80211_hw_check(&tx->local->hw, QUEUE_CONTROL))
431 		info->hw_queue = tx->sdata->vif.cab_queue;
432 
433 	/* no stations in PS mode and no buffered packets */
434 	if (!atomic_read(&ps->num_sta_ps) && skb_queue_empty(&ps->bc_buf))
435 		return TX_CONTINUE;
436 
437 	info->flags |= IEEE80211_TX_CTL_SEND_AFTER_DTIM;
438 
439 	/* device releases frame after DTIM beacon */
440 	if (!ieee80211_hw_check(&tx->local->hw, HOST_BROADCAST_PS_BUFFERING))
441 		return TX_CONTINUE;
442 
443 	/* buffered in mac80211 */
444 	if (tx->local->total_ps_buffered >= TOTAL_MAX_TX_BUFFER)
445 		purge_old_ps_buffers(tx->local);
446 
447 	if (skb_queue_len(&ps->bc_buf) >= AP_MAX_BC_BUFFER) {
448 		ps_dbg(tx->sdata,
449 		       "BC TX buffer full - dropping the oldest frame\n");
450 		ieee80211_free_txskb(&tx->local->hw, skb_dequeue(&ps->bc_buf));
451 	} else
452 		tx->local->total_ps_buffered++;
453 
454 	skb_queue_tail(&ps->bc_buf, tx->skb);
455 
456 	return TX_QUEUED;
457 }
458 
ieee80211_use_mfp(__le16 fc,struct sta_info * sta,struct sk_buff * skb)459 static int ieee80211_use_mfp(__le16 fc, struct sta_info *sta,
460 			     struct sk_buff *skb)
461 {
462 	if (!ieee80211_is_mgmt(fc))
463 		return 0;
464 
465 	if (sta == NULL || !test_sta_flag(sta, WLAN_STA_MFP))
466 		return 0;
467 
468 	if (!ieee80211_is_robust_mgmt_frame(skb))
469 		return 0;
470 
471 	return 1;
472 }
473 
474 static ieee80211_tx_result
ieee80211_tx_h_unicast_ps_buf(struct ieee80211_tx_data * tx)475 ieee80211_tx_h_unicast_ps_buf(struct ieee80211_tx_data *tx)
476 {
477 	struct sta_info *sta = tx->sta;
478 	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(tx->skb);
479 	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)tx->skb->data;
480 	struct ieee80211_local *local = tx->local;
481 
482 	if (unlikely(!sta))
483 		return TX_CONTINUE;
484 
485 	if (unlikely((test_sta_flag(sta, WLAN_STA_PS_STA) ||
486 		      test_sta_flag(sta, WLAN_STA_PS_DRIVER) ||
487 		      test_sta_flag(sta, WLAN_STA_PS_DELIVER)) &&
488 		     !(info->flags & IEEE80211_TX_CTL_NO_PS_BUFFER))) {
489 		int ac = skb_get_queue_mapping(tx->skb);
490 
491 		if (ieee80211_is_mgmt(hdr->frame_control) &&
492 		    !ieee80211_is_bufferable_mmpdu(tx->skb)) {
493 			info->flags |= IEEE80211_TX_CTL_NO_PS_BUFFER;
494 			return TX_CONTINUE;
495 		}
496 
497 		ps_dbg(sta->sdata, "STA %pM aid %d: PS buffer for AC %d\n",
498 		       sta->sta.addr, sta->sta.aid, ac);
499 		if (tx->local->total_ps_buffered >= TOTAL_MAX_TX_BUFFER)
500 			purge_old_ps_buffers(tx->local);
501 
502 		/* sync with ieee80211_sta_ps_deliver_wakeup */
503 		spin_lock(&sta->ps_lock);
504 		/*
505 		 * STA woke up the meantime and all the frames on ps_tx_buf have
506 		 * been queued to pending queue. No reordering can happen, go
507 		 * ahead and Tx the packet.
508 		 */
509 		if (!test_sta_flag(sta, WLAN_STA_PS_STA) &&
510 		    !test_sta_flag(sta, WLAN_STA_PS_DRIVER) &&
511 		    !test_sta_flag(sta, WLAN_STA_PS_DELIVER)) {
512 			spin_unlock(&sta->ps_lock);
513 			return TX_CONTINUE;
514 		}
515 
516 		if (skb_queue_len(&sta->ps_tx_buf[ac]) >= STA_MAX_TX_BUFFER) {
517 			struct sk_buff *old = skb_dequeue(&sta->ps_tx_buf[ac]);
518 			ps_dbg(tx->sdata,
519 			       "STA %pM TX buffer for AC %d full - dropping oldest frame\n",
520 			       sta->sta.addr, ac);
521 			ieee80211_free_txskb(&local->hw, old);
522 		} else
523 			tx->local->total_ps_buffered++;
524 
525 		info->control.jiffies = jiffies;
526 		info->control.vif = &tx->sdata->vif;
527 		info->control.flags |= IEEE80211_TX_INTCFL_NEED_TXPROCESSING;
528 		info->flags &= ~IEEE80211_TX_TEMPORARY_FLAGS;
529 		skb_queue_tail(&sta->ps_tx_buf[ac], tx->skb);
530 		spin_unlock(&sta->ps_lock);
531 
532 		if (!timer_pending(&local->sta_cleanup))
533 			mod_timer(&local->sta_cleanup,
534 				  round_jiffies(jiffies +
535 						STA_INFO_CLEANUP_INTERVAL));
536 
537 		/*
538 		 * We queued up some frames, so the TIM bit might
539 		 * need to be set, recalculate it.
540 		 */
541 		sta_info_recalc_tim(sta);
542 
543 		return TX_QUEUED;
544 	} else if (unlikely(test_sta_flag(sta, WLAN_STA_PS_STA))) {
545 		ps_dbg(tx->sdata,
546 		       "STA %pM in PS mode, but polling/in SP -> send frame\n",
547 		       sta->sta.addr);
548 	}
549 
550 	return TX_CONTINUE;
551 }
552 
553 static ieee80211_tx_result debug_noinline
ieee80211_tx_h_ps_buf(struct ieee80211_tx_data * tx)554 ieee80211_tx_h_ps_buf(struct ieee80211_tx_data *tx)
555 {
556 	if (unlikely(tx->flags & IEEE80211_TX_PS_BUFFERED))
557 		return TX_CONTINUE;
558 
559 	if (tx->flags & IEEE80211_TX_UNICAST)
560 		return ieee80211_tx_h_unicast_ps_buf(tx);
561 	else
562 		return ieee80211_tx_h_multicast_ps_buf(tx);
563 }
564 
565 static ieee80211_tx_result debug_noinline
ieee80211_tx_h_check_control_port_protocol(struct ieee80211_tx_data * tx)566 ieee80211_tx_h_check_control_port_protocol(struct ieee80211_tx_data *tx)
567 {
568 	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(tx->skb);
569 
570 	if (unlikely(tx->sdata->control_port_protocol == tx->skb->protocol)) {
571 		if (tx->sdata->control_port_no_encrypt)
572 			info->flags |= IEEE80211_TX_INTFL_DONT_ENCRYPT;
573 		info->control.flags |= IEEE80211_TX_CTRL_PORT_CTRL_PROTO;
574 		info->flags |= IEEE80211_TX_CTL_USE_MINRATE;
575 	}
576 
577 	return TX_CONTINUE;
578 }
579 
580 static struct ieee80211_key *
ieee80211_select_link_key(struct ieee80211_tx_data * tx)581 ieee80211_select_link_key(struct ieee80211_tx_data *tx)
582 {
583 	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)tx->skb->data;
584 	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(tx->skb);
585 	struct ieee80211_link_data *link;
586 	unsigned int link_id;
587 
588 	link_id = u32_get_bits(info->control.flags, IEEE80211_TX_CTRL_MLO_LINK);
589 	if (link_id == IEEE80211_LINK_UNSPECIFIED) {
590 		link = &tx->sdata->deflink;
591 	} else {
592 		link = rcu_dereference(tx->sdata->link[link_id]);
593 		if (!link)
594 			return NULL;
595 	}
596 
597 	if (ieee80211_is_group_privacy_action(tx->skb))
598 		return rcu_dereference(link->default_multicast_key);
599 	else if (ieee80211_is_mgmt(hdr->frame_control) &&
600 		 is_multicast_ether_addr(hdr->addr1) &&
601 		 ieee80211_is_robust_mgmt_frame(tx->skb))
602 		return rcu_dereference(link->default_mgmt_key);
603 	else if (is_multicast_ether_addr(hdr->addr1))
604 		return rcu_dereference(link->default_multicast_key);
605 
606 	return NULL;
607 }
608 
609 static ieee80211_tx_result debug_noinline
ieee80211_tx_h_select_key(struct ieee80211_tx_data * tx)610 ieee80211_tx_h_select_key(struct ieee80211_tx_data *tx)
611 {
612 	struct ieee80211_key *key;
613 	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(tx->skb);
614 	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)tx->skb->data;
615 
616 	if (unlikely(info->flags & IEEE80211_TX_INTFL_DONT_ENCRYPT)) {
617 		tx->key = NULL;
618 		return TX_CONTINUE;
619 	}
620 
621 	if (tx->sta &&
622 	    (key = rcu_dereference(tx->sta->ptk[tx->sta->ptk_idx])))
623 		tx->key = key;
624 	else if ((key = ieee80211_select_link_key(tx)))
625 		tx->key = key;
626 	else if (!is_multicast_ether_addr(hdr->addr1) &&
627 		 (key = rcu_dereference(tx->sdata->default_unicast_key)))
628 		tx->key = key;
629 	else
630 		tx->key = NULL;
631 
632 	if (tx->key) {
633 		bool skip_hw = false;
634 
635 		/* TODO: add threshold stuff again */
636 
637 		switch (tx->key->conf.cipher) {
638 		case WLAN_CIPHER_SUITE_WEP40:
639 		case WLAN_CIPHER_SUITE_WEP104:
640 		case WLAN_CIPHER_SUITE_TKIP:
641 			if (!ieee80211_is_data_present(hdr->frame_control))
642 				tx->key = NULL;
643 			break;
644 		case WLAN_CIPHER_SUITE_CCMP:
645 		case WLAN_CIPHER_SUITE_CCMP_256:
646 		case WLAN_CIPHER_SUITE_GCMP:
647 		case WLAN_CIPHER_SUITE_GCMP_256:
648 			if (!ieee80211_is_data_present(hdr->frame_control) &&
649 			    !ieee80211_use_mfp(hdr->frame_control, tx->sta,
650 					       tx->skb) &&
651 			    !ieee80211_is_group_privacy_action(tx->skb))
652 				tx->key = NULL;
653 			else
654 				skip_hw = (tx->key->conf.flags &
655 					   IEEE80211_KEY_FLAG_SW_MGMT_TX) &&
656 					ieee80211_is_mgmt(hdr->frame_control);
657 			break;
658 		case WLAN_CIPHER_SUITE_AES_CMAC:
659 		case WLAN_CIPHER_SUITE_BIP_CMAC_256:
660 		case WLAN_CIPHER_SUITE_BIP_GMAC_128:
661 		case WLAN_CIPHER_SUITE_BIP_GMAC_256:
662 			if (!ieee80211_is_mgmt(hdr->frame_control))
663 				tx->key = NULL;
664 			break;
665 		}
666 
667 		if (unlikely(tx->key && tx->key->flags & KEY_FLAG_TAINTED &&
668 			     !ieee80211_is_deauth(hdr->frame_control)) &&
669 			     tx->skb->protocol != tx->sdata->control_port_protocol)
670 			return TX_DROP;
671 
672 		if (!skip_hw && tx->key &&
673 		    tx->key->flags & KEY_FLAG_UPLOADED_TO_HARDWARE)
674 			info->control.hw_key = &tx->key->conf;
675 	} else if (ieee80211_is_data_present(hdr->frame_control) && tx->sta &&
676 		   test_sta_flag(tx->sta, WLAN_STA_USES_ENCRYPTION)) {
677 		return TX_DROP;
678 	}
679 
680 	return TX_CONTINUE;
681 }
682 
683 static ieee80211_tx_result debug_noinline
ieee80211_tx_h_rate_ctrl(struct ieee80211_tx_data * tx)684 ieee80211_tx_h_rate_ctrl(struct ieee80211_tx_data *tx)
685 {
686 	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(tx->skb);
687 	struct ieee80211_hdr *hdr = (void *)tx->skb->data;
688 	struct ieee80211_supported_band *sband;
689 	u32 len;
690 	struct ieee80211_tx_rate_control txrc;
691 	struct ieee80211_sta_rates *ratetbl = NULL;
692 	bool encap = info->flags & IEEE80211_TX_CTL_HW_80211_ENCAP;
693 	bool assoc = false;
694 
695 	memset(&txrc, 0, sizeof(txrc));
696 
697 	sband = tx->local->hw.wiphy->bands[info->band];
698 
699 	len = min_t(u32, tx->skb->len + FCS_LEN,
700 			 tx->local->hw.wiphy->frag_threshold);
701 
702 	/* set up the tx rate control struct we give the RC algo */
703 	txrc.hw = &tx->local->hw;
704 	txrc.sband = sband;
705 	txrc.bss_conf = &tx->sdata->vif.bss_conf;
706 	txrc.skb = tx->skb;
707 	txrc.reported_rate.idx = -1;
708 
709 	if (unlikely(info->control.flags & IEEE80211_TX_CTRL_DONT_USE_RATE_MASK)) {
710 		txrc.rate_idx_mask = ~0;
711 	} else {
712 		txrc.rate_idx_mask = tx->sdata->rc_rateidx_mask[info->band];
713 
714 		if (tx->sdata->rc_has_mcs_mask[info->band])
715 			txrc.rate_idx_mcs_mask =
716 				tx->sdata->rc_rateidx_mcs_mask[info->band];
717 	}
718 
719 	txrc.bss = (tx->sdata->vif.type == NL80211_IFTYPE_AP ||
720 		    tx->sdata->vif.type == NL80211_IFTYPE_MESH_POINT ||
721 		    tx->sdata->vif.type == NL80211_IFTYPE_ADHOC ||
722 		    tx->sdata->vif.type == NL80211_IFTYPE_OCB);
723 
724 	/* set up RTS protection if desired */
725 	if (len > tx->local->hw.wiphy->rts_threshold) {
726 		txrc.rts = true;
727 	}
728 
729 	info->control.use_rts = txrc.rts;
730 	info->control.use_cts_prot = tx->sdata->vif.bss_conf.use_cts_prot;
731 
732 	/*
733 	 * Use short preamble if the BSS can handle it, but not for
734 	 * management frames unless we know the receiver can handle
735 	 * that -- the management frame might be to a station that
736 	 * just wants a probe response.
737 	 */
738 	if (tx->sdata->vif.bss_conf.use_short_preamble &&
739 	    (ieee80211_is_tx_data(tx->skb) ||
740 	     (tx->sta && test_sta_flag(tx->sta, WLAN_STA_SHORT_PREAMBLE))))
741 		txrc.short_preamble = true;
742 
743 	info->control.short_preamble = txrc.short_preamble;
744 
745 	/* don't ask rate control when rate already injected via radiotap */
746 	if (info->control.flags & IEEE80211_TX_CTRL_RATE_INJECT)
747 		return TX_CONTINUE;
748 
749 	if (tx->sta)
750 		assoc = test_sta_flag(tx->sta, WLAN_STA_ASSOC);
751 
752 	/*
753 	 * Lets not bother rate control if we're associated and cannot
754 	 * talk to the sta. This should not happen.
755 	 */
756 	if (WARN(test_bit(SCAN_SW_SCANNING, &tx->local->scanning) && assoc &&
757 		 !rate_usable_index_exists(sband, &tx->sta->sta),
758 		 "%s: Dropped data frame as no usable bitrate found while "
759 		 "scanning and associated. Target station: "
760 		 "%pM on %d GHz band\n",
761 		 tx->sdata->name,
762 		 encap ? ((struct ethhdr *)hdr)->h_dest : hdr->addr1,
763 		 info->band ? 5 : 2))
764 		return TX_DROP;
765 
766 	/*
767 	 * If we're associated with the sta at this point we know we can at
768 	 * least send the frame at the lowest bit rate.
769 	 */
770 	rate_control_get_rate(tx->sdata, tx->sta, &txrc);
771 
772 	if (tx->sta && !info->control.skip_table)
773 		ratetbl = rcu_dereference(tx->sta->sta.rates);
774 
775 	if (unlikely(info->control.rates[0].idx < 0)) {
776 		if (ratetbl) {
777 			struct ieee80211_tx_rate rate = {
778 				.idx = ratetbl->rate[0].idx,
779 				.flags = ratetbl->rate[0].flags,
780 				.count = ratetbl->rate[0].count
781 			};
782 
783 			if (ratetbl->rate[0].idx < 0)
784 				return TX_DROP;
785 
786 			tx->rate = rate;
787 		} else {
788 			return TX_DROP;
789 		}
790 	} else {
791 		tx->rate = info->control.rates[0];
792 	}
793 
794 	if (txrc.reported_rate.idx < 0) {
795 		txrc.reported_rate = tx->rate;
796 		if (tx->sta && ieee80211_is_tx_data(tx->skb))
797 			tx->sta->deflink.tx_stats.last_rate = txrc.reported_rate;
798 	} else if (tx->sta)
799 		tx->sta->deflink.tx_stats.last_rate = txrc.reported_rate;
800 
801 	if (ratetbl)
802 		return TX_CONTINUE;
803 
804 	if (unlikely(!info->control.rates[0].count))
805 		info->control.rates[0].count = 1;
806 
807 	if (WARN_ON_ONCE((info->control.rates[0].count > 1) &&
808 			 (info->flags & IEEE80211_TX_CTL_NO_ACK)))
809 		info->control.rates[0].count = 1;
810 
811 	return TX_CONTINUE;
812 }
813 
ieee80211_tx_next_seq(struct sta_info * sta,int tid)814 static __le16 ieee80211_tx_next_seq(struct sta_info *sta, int tid)
815 {
816 	u16 *seq = &sta->tid_seq[tid];
817 	__le16 ret = cpu_to_le16(*seq);
818 
819 	/* Increase the sequence number. */
820 	*seq = (*seq + 0x10) & IEEE80211_SCTL_SEQ;
821 
822 	return ret;
823 }
824 
825 static ieee80211_tx_result debug_noinline
ieee80211_tx_h_sequence(struct ieee80211_tx_data * tx)826 ieee80211_tx_h_sequence(struct ieee80211_tx_data *tx)
827 {
828 	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(tx->skb);
829 	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)tx->skb->data;
830 	int tid;
831 
832 	/*
833 	 * Packet injection may want to control the sequence
834 	 * number, if we have no matching interface then we
835 	 * neither assign one ourselves nor ask the driver to.
836 	 */
837 	if (unlikely(info->control.vif->type == NL80211_IFTYPE_MONITOR))
838 		return TX_CONTINUE;
839 
840 	if (unlikely(ieee80211_is_ctl(hdr->frame_control)))
841 		return TX_CONTINUE;
842 
843 	if (ieee80211_hdrlen(hdr->frame_control) < 24)
844 		return TX_CONTINUE;
845 
846 	if (ieee80211_is_qos_nullfunc(hdr->frame_control))
847 		return TX_CONTINUE;
848 
849 	if (info->control.flags & IEEE80211_TX_CTRL_NO_SEQNO)
850 		return TX_CONTINUE;
851 
852 	/* SNS11 from 802.11be 10.3.2.14 */
853 	if (unlikely(is_multicast_ether_addr(hdr->addr1) &&
854 		     ieee80211_vif_is_mld(info->control.vif) &&
855 		     info->control.vif->type == NL80211_IFTYPE_AP)) {
856 		if (info->control.flags & IEEE80211_TX_CTRL_MCAST_MLO_FIRST_TX)
857 			tx->sdata->mld_mcast_seq += 0x10;
858 		hdr->seq_ctrl = cpu_to_le16(tx->sdata->mld_mcast_seq);
859 		return TX_CONTINUE;
860 	}
861 
862 	/*
863 	 * Anything but QoS data that has a sequence number field
864 	 * (is long enough) gets a sequence number from the global
865 	 * counter.  QoS data frames with a multicast destination
866 	 * also use the global counter (802.11-2012 9.3.2.10).
867 	 */
868 	if (!ieee80211_is_data_qos(hdr->frame_control) ||
869 	    is_multicast_ether_addr(hdr->addr1)) {
870 		/* driver should assign sequence number */
871 		info->flags |= IEEE80211_TX_CTL_ASSIGN_SEQ;
872 		/* for pure STA mode without beacons, we can do it */
873 		hdr->seq_ctrl = cpu_to_le16(tx->sdata->sequence_number);
874 		tx->sdata->sequence_number += 0x10;
875 		if (tx->sta)
876 			tx->sta->deflink.tx_stats.msdu[IEEE80211_NUM_TIDS]++;
877 		return TX_CONTINUE;
878 	}
879 
880 	/*
881 	 * This should be true for injected/management frames only, for
882 	 * management frames we have set the IEEE80211_TX_CTL_ASSIGN_SEQ
883 	 * above since they are not QoS-data frames.
884 	 */
885 	if (!tx->sta)
886 		return TX_CONTINUE;
887 
888 	/* include per-STA, per-TID sequence counter */
889 	tid = ieee80211_get_tid(hdr);
890 	tx->sta->deflink.tx_stats.msdu[tid]++;
891 
892 	hdr->seq_ctrl = ieee80211_tx_next_seq(tx->sta, tid);
893 
894 	return TX_CONTINUE;
895 }
896 
ieee80211_fragment(struct ieee80211_tx_data * tx,struct sk_buff * skb,int hdrlen,int frag_threshold)897 static int ieee80211_fragment(struct ieee80211_tx_data *tx,
898 			      struct sk_buff *skb, int hdrlen,
899 			      int frag_threshold)
900 {
901 	struct ieee80211_local *local = tx->local;
902 	struct ieee80211_tx_info *info;
903 	struct sk_buff *tmp;
904 	int per_fragm = frag_threshold - hdrlen - FCS_LEN;
905 	int pos = hdrlen + per_fragm;
906 	int rem = skb->len - hdrlen - per_fragm;
907 
908 	if (WARN_ON(rem < 0))
909 		return -EINVAL;
910 
911 	/* first fragment was already added to queue by caller */
912 
913 	while (rem) {
914 		int fraglen = per_fragm;
915 
916 		if (fraglen > rem)
917 			fraglen = rem;
918 		rem -= fraglen;
919 		tmp = dev_alloc_skb(local->tx_headroom +
920 				    frag_threshold +
921 				    IEEE80211_ENCRYPT_HEADROOM +
922 				    IEEE80211_ENCRYPT_TAILROOM);
923 		if (!tmp)
924 			return -ENOMEM;
925 
926 		__skb_queue_tail(&tx->skbs, tmp);
927 
928 		skb_reserve(tmp,
929 			    local->tx_headroom + IEEE80211_ENCRYPT_HEADROOM);
930 
931 		/* copy control information */
932 		memcpy(tmp->cb, skb->cb, sizeof(tmp->cb));
933 
934 		info = IEEE80211_SKB_CB(tmp);
935 		info->flags &= ~(IEEE80211_TX_CTL_CLEAR_PS_FILT |
936 				 IEEE80211_TX_CTL_FIRST_FRAGMENT);
937 
938 		if (rem)
939 			info->flags |= IEEE80211_TX_CTL_MORE_FRAMES;
940 
941 		skb_copy_queue_mapping(tmp, skb);
942 		tmp->priority = skb->priority;
943 		tmp->dev = skb->dev;
944 
945 		/* copy header and data */
946 		skb_put_data(tmp, skb->data, hdrlen);
947 		skb_put_data(tmp, skb->data + pos, fraglen);
948 
949 		pos += fraglen;
950 	}
951 
952 	/* adjust first fragment's length */
953 	skb_trim(skb, hdrlen + per_fragm);
954 	return 0;
955 }
956 
957 static ieee80211_tx_result debug_noinline
ieee80211_tx_h_fragment(struct ieee80211_tx_data * tx)958 ieee80211_tx_h_fragment(struct ieee80211_tx_data *tx)
959 {
960 	struct sk_buff *skb = tx->skb;
961 	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
962 	struct ieee80211_hdr *hdr = (void *)skb->data;
963 	int frag_threshold = tx->local->hw.wiphy->frag_threshold;
964 	int hdrlen;
965 	int fragnum;
966 
967 	/* no matter what happens, tx->skb moves to tx->skbs */
968 	__skb_queue_tail(&tx->skbs, skb);
969 	tx->skb = NULL;
970 
971 	if (info->flags & IEEE80211_TX_CTL_DONTFRAG)
972 		return TX_CONTINUE;
973 
974 	if (ieee80211_hw_check(&tx->local->hw, SUPPORTS_TX_FRAG))
975 		return TX_CONTINUE;
976 
977 	/*
978 	 * Warn when submitting a fragmented A-MPDU frame and drop it.
979 	 * This scenario is handled in ieee80211_tx_prepare but extra
980 	 * caution taken here as fragmented ampdu may cause Tx stop.
981 	 */
982 	if (WARN_ON(info->flags & IEEE80211_TX_CTL_AMPDU))
983 		return TX_DROP;
984 
985 	hdrlen = ieee80211_hdrlen(hdr->frame_control);
986 
987 	/* internal error, why isn't DONTFRAG set? */
988 	if (WARN_ON(skb->len + FCS_LEN <= frag_threshold))
989 		return TX_DROP;
990 
991 	/*
992 	 * Now fragment the frame. This will allocate all the fragments and
993 	 * chain them (using skb as the first fragment) to skb->next.
994 	 * During transmission, we will remove the successfully transmitted
995 	 * fragments from this list. When the low-level driver rejects one
996 	 * of the fragments then we will simply pretend to accept the skb
997 	 * but store it away as pending.
998 	 */
999 	if (ieee80211_fragment(tx, skb, hdrlen, frag_threshold))
1000 		return TX_DROP;
1001 
1002 	/* update duration/seq/flags of fragments */
1003 	fragnum = 0;
1004 
1005 	skb_queue_walk(&tx->skbs, skb) {
1006 		const __le16 morefrags = cpu_to_le16(IEEE80211_FCTL_MOREFRAGS);
1007 
1008 		hdr = (void *)skb->data;
1009 		info = IEEE80211_SKB_CB(skb);
1010 
1011 		if (!skb_queue_is_last(&tx->skbs, skb)) {
1012 			hdr->frame_control |= morefrags;
1013 			/*
1014 			 * No multi-rate retries for fragmented frames, that
1015 			 * would completely throw off the NAV at other STAs.
1016 			 */
1017 			info->control.rates[1].idx = -1;
1018 			info->control.rates[2].idx = -1;
1019 			info->control.rates[3].idx = -1;
1020 			BUILD_BUG_ON(IEEE80211_TX_MAX_RATES != 4);
1021 			info->flags &= ~IEEE80211_TX_CTL_RATE_CTRL_PROBE;
1022 		} else {
1023 			hdr->frame_control &= ~morefrags;
1024 		}
1025 		hdr->seq_ctrl |= cpu_to_le16(fragnum & IEEE80211_SCTL_FRAG);
1026 		fragnum++;
1027 	}
1028 
1029 	return TX_CONTINUE;
1030 }
1031 
1032 static ieee80211_tx_result debug_noinline
ieee80211_tx_h_stats(struct ieee80211_tx_data * tx)1033 ieee80211_tx_h_stats(struct ieee80211_tx_data *tx)
1034 {
1035 	struct sk_buff *skb;
1036 	int ac = -1;
1037 
1038 	if (!tx->sta)
1039 		return TX_CONTINUE;
1040 
1041 	skb_queue_walk(&tx->skbs, skb) {
1042 		ac = skb_get_queue_mapping(skb);
1043 		tx->sta->deflink.tx_stats.bytes[ac] += skb->len;
1044 	}
1045 	if (ac >= 0)
1046 		tx->sta->deflink.tx_stats.packets[ac]++;
1047 
1048 	return TX_CONTINUE;
1049 }
1050 
1051 static ieee80211_tx_result debug_noinline
ieee80211_tx_h_encrypt(struct ieee80211_tx_data * tx)1052 ieee80211_tx_h_encrypt(struct ieee80211_tx_data *tx)
1053 {
1054 	if (!tx->key)
1055 		return TX_CONTINUE;
1056 
1057 	switch (tx->key->conf.cipher) {
1058 	case WLAN_CIPHER_SUITE_WEP40:
1059 	case WLAN_CIPHER_SUITE_WEP104:
1060 		return ieee80211_crypto_wep_encrypt(tx);
1061 	case WLAN_CIPHER_SUITE_TKIP:
1062 		return ieee80211_crypto_tkip_encrypt(tx);
1063 	case WLAN_CIPHER_SUITE_CCMP:
1064 		return ieee80211_crypto_ccmp_encrypt(
1065 			tx, IEEE80211_CCMP_MIC_LEN);
1066 	case WLAN_CIPHER_SUITE_CCMP_256:
1067 		return ieee80211_crypto_ccmp_encrypt(
1068 			tx, IEEE80211_CCMP_256_MIC_LEN);
1069 	case WLAN_CIPHER_SUITE_AES_CMAC:
1070 		return ieee80211_crypto_aes_cmac_encrypt(tx);
1071 	case WLAN_CIPHER_SUITE_BIP_CMAC_256:
1072 		return ieee80211_crypto_aes_cmac_256_encrypt(tx);
1073 	case WLAN_CIPHER_SUITE_BIP_GMAC_128:
1074 	case WLAN_CIPHER_SUITE_BIP_GMAC_256:
1075 		return ieee80211_crypto_aes_gmac_encrypt(tx);
1076 	case WLAN_CIPHER_SUITE_GCMP:
1077 	case WLAN_CIPHER_SUITE_GCMP_256:
1078 		return ieee80211_crypto_gcmp_encrypt(tx);
1079 	}
1080 
1081 	return TX_DROP;
1082 }
1083 
1084 static ieee80211_tx_result debug_noinline
ieee80211_tx_h_calculate_duration(struct ieee80211_tx_data * tx)1085 ieee80211_tx_h_calculate_duration(struct ieee80211_tx_data *tx)
1086 {
1087 	struct sk_buff *skb;
1088 	struct ieee80211_hdr *hdr;
1089 	int next_len;
1090 	bool group_addr;
1091 
1092 	skb_queue_walk(&tx->skbs, skb) {
1093 		hdr = (void *) skb->data;
1094 		if (unlikely(ieee80211_is_pspoll(hdr->frame_control)))
1095 			break; /* must not overwrite AID */
1096 		if (!skb_queue_is_last(&tx->skbs, skb)) {
1097 			struct sk_buff *next = skb_queue_next(&tx->skbs, skb);
1098 			next_len = next->len;
1099 		} else
1100 			next_len = 0;
1101 		group_addr = is_multicast_ether_addr(hdr->addr1);
1102 
1103 		hdr->duration_id =
1104 			ieee80211_duration(tx, skb, group_addr, next_len);
1105 	}
1106 
1107 	return TX_CONTINUE;
1108 }
1109 
1110 /* actual transmit path */
1111 
ieee80211_tx_prep_agg(struct ieee80211_tx_data * tx,struct sk_buff * skb,struct ieee80211_tx_info * info,struct tid_ampdu_tx * tid_tx,int tid)1112 static bool ieee80211_tx_prep_agg(struct ieee80211_tx_data *tx,
1113 				  struct sk_buff *skb,
1114 				  struct ieee80211_tx_info *info,
1115 				  struct tid_ampdu_tx *tid_tx,
1116 				  int tid)
1117 {
1118 	bool queued = false;
1119 	bool reset_agg_timer = false;
1120 	struct sk_buff *purge_skb = NULL;
1121 
1122 	if (test_bit(HT_AGG_STATE_OPERATIONAL, &tid_tx->state)) {
1123 		reset_agg_timer = true;
1124 	} else if (test_bit(HT_AGG_STATE_WANT_START, &tid_tx->state)) {
1125 		/*
1126 		 * nothing -- this aggregation session is being started
1127 		 * but that might still fail with the driver
1128 		 */
1129 	} else if (!tx->sta->sta.txq[tid]) {
1130 		spin_lock(&tx->sta->lock);
1131 		/*
1132 		 * Need to re-check now, because we may get here
1133 		 *
1134 		 *  1) in the window during which the setup is actually
1135 		 *     already done, but not marked yet because not all
1136 		 *     packets are spliced over to the driver pending
1137 		 *     queue yet -- if this happened we acquire the lock
1138 		 *     either before or after the splice happens, but
1139 		 *     need to recheck which of these cases happened.
1140 		 *
1141 		 *  2) during session teardown, if the OPERATIONAL bit
1142 		 *     was cleared due to the teardown but the pointer
1143 		 *     hasn't been assigned NULL yet (or we loaded it
1144 		 *     before it was assigned) -- in this case it may
1145 		 *     now be NULL which means we should just let the
1146 		 *     packet pass through because splicing the frames
1147 		 *     back is already done.
1148 		 */
1149 		tid_tx = rcu_dereference_protected_tid_tx(tx->sta, tid);
1150 
1151 		if (!tid_tx) {
1152 			/* do nothing, let packet pass through */
1153 		} else if (test_bit(HT_AGG_STATE_OPERATIONAL, &tid_tx->state)) {
1154 			reset_agg_timer = true;
1155 		} else {
1156 			queued = true;
1157 			if (info->flags & IEEE80211_TX_CTL_NO_PS_BUFFER) {
1158 				clear_sta_flag(tx->sta, WLAN_STA_SP);
1159 				ps_dbg(tx->sta->sdata,
1160 				       "STA %pM aid %d: SP frame queued, close the SP w/o telling the peer\n",
1161 				       tx->sta->sta.addr, tx->sta->sta.aid);
1162 			}
1163 			info->control.vif = &tx->sdata->vif;
1164 			info->control.flags |= IEEE80211_TX_INTCFL_NEED_TXPROCESSING;
1165 			info->flags &= ~IEEE80211_TX_TEMPORARY_FLAGS;
1166 			__skb_queue_tail(&tid_tx->pending, skb);
1167 			if (skb_queue_len(&tid_tx->pending) > STA_MAX_TX_BUFFER)
1168 				purge_skb = __skb_dequeue(&tid_tx->pending);
1169 		}
1170 		spin_unlock(&tx->sta->lock);
1171 
1172 		if (purge_skb)
1173 			ieee80211_free_txskb(&tx->local->hw, purge_skb);
1174 	}
1175 
1176 	/* reset session timer */
1177 	if (reset_agg_timer)
1178 		tid_tx->last_tx = jiffies;
1179 
1180 	return queued;
1181 }
1182 
ieee80211_aggr_check(struct ieee80211_sub_if_data * sdata,struct sta_info * sta,struct sk_buff * skb)1183 void ieee80211_aggr_check(struct ieee80211_sub_if_data *sdata,
1184 			  struct sta_info *sta, struct sk_buff *skb)
1185 {
1186 	struct rate_control_ref *ref = sdata->local->rate_ctrl;
1187 	u16 tid;
1188 
1189 	if (!ref || !(ref->ops->capa & RATE_CTRL_CAPA_AMPDU_TRIGGER))
1190 		return;
1191 
1192 	if (!sta || !sta->sta.deflink.ht_cap.ht_supported ||
1193 	    !sta->sta.wme || skb_get_queue_mapping(skb) == IEEE80211_AC_VO ||
1194 	    skb->protocol == sdata->control_port_protocol)
1195 		return;
1196 
1197 	tid = skb->priority & IEEE80211_QOS_CTL_TID_MASK;
1198 	if (likely(sta->ampdu_mlme.tid_tx[tid]))
1199 		return;
1200 
1201 	ieee80211_start_tx_ba_session(&sta->sta, tid, 0);
1202 }
1203 
1204 /*
1205  * initialises @tx
1206  * pass %NULL for the station if unknown, a valid pointer if known
1207  * or an ERR_PTR() if the station is known not to exist
1208  */
1209 static ieee80211_tx_result
ieee80211_tx_prepare(struct ieee80211_sub_if_data * sdata,struct ieee80211_tx_data * tx,struct sta_info * sta,struct sk_buff * skb)1210 ieee80211_tx_prepare(struct ieee80211_sub_if_data *sdata,
1211 		     struct ieee80211_tx_data *tx,
1212 		     struct sta_info *sta, struct sk_buff *skb)
1213 {
1214 	struct ieee80211_local *local = sdata->local;
1215 	struct ieee80211_hdr *hdr;
1216 	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
1217 	bool aggr_check = false;
1218 	int tid;
1219 
1220 	memset(tx, 0, sizeof(*tx));
1221 	tx->skb = skb;
1222 	tx->local = local;
1223 	tx->sdata = sdata;
1224 	__skb_queue_head_init(&tx->skbs);
1225 
1226 	/*
1227 	 * If this flag is set to true anywhere, and we get here,
1228 	 * we are doing the needed processing, so remove the flag
1229 	 * now.
1230 	 */
1231 	info->control.flags &= ~IEEE80211_TX_INTCFL_NEED_TXPROCESSING;
1232 
1233 	hdr = (struct ieee80211_hdr *) skb->data;
1234 
1235 	if (likely(sta)) {
1236 		if (!IS_ERR(sta))
1237 			tx->sta = sta;
1238 	} else {
1239 		if (sdata->vif.type == NL80211_IFTYPE_AP_VLAN) {
1240 			tx->sta = rcu_dereference(sdata->u.vlan.sta);
1241 			if (!tx->sta && sdata->wdev.use_4addr)
1242 				return TX_DROP;
1243 		} else if (tx->sdata->control_port_protocol == tx->skb->protocol) {
1244 			tx->sta = sta_info_get_bss(sdata, hdr->addr1);
1245 		}
1246 		if (!tx->sta && !is_multicast_ether_addr(hdr->addr1)) {
1247 			tx->sta = sta_info_get(sdata, hdr->addr1);
1248 			aggr_check = true;
1249 		}
1250 	}
1251 
1252 	if (tx->sta && ieee80211_is_data_qos(hdr->frame_control) &&
1253 	    !ieee80211_is_qos_nullfunc(hdr->frame_control) &&
1254 	    ieee80211_hw_check(&local->hw, AMPDU_AGGREGATION) &&
1255 	    !ieee80211_hw_check(&local->hw, TX_AMPDU_SETUP_IN_HW)) {
1256 		struct tid_ampdu_tx *tid_tx;
1257 
1258 		tid = ieee80211_get_tid(hdr);
1259 		tid_tx = rcu_dereference(tx->sta->ampdu_mlme.tid_tx[tid]);
1260 		if (!tid_tx && aggr_check) {
1261 			ieee80211_aggr_check(sdata, tx->sta, skb);
1262 			tid_tx = rcu_dereference(tx->sta->ampdu_mlme.tid_tx[tid]);
1263 		}
1264 
1265 		if (tid_tx) {
1266 			bool queued;
1267 
1268 			queued = ieee80211_tx_prep_agg(tx, skb, info,
1269 						       tid_tx, tid);
1270 
1271 			if (unlikely(queued))
1272 				return TX_QUEUED;
1273 		}
1274 	}
1275 
1276 	if (is_multicast_ether_addr(hdr->addr1)) {
1277 		tx->flags &= ~IEEE80211_TX_UNICAST;
1278 		info->flags |= IEEE80211_TX_CTL_NO_ACK;
1279 	} else
1280 		tx->flags |= IEEE80211_TX_UNICAST;
1281 
1282 	if (!(info->flags & IEEE80211_TX_CTL_DONTFRAG)) {
1283 		if (!(tx->flags & IEEE80211_TX_UNICAST) ||
1284 		    skb->len + FCS_LEN <= local->hw.wiphy->frag_threshold ||
1285 		    info->flags & IEEE80211_TX_CTL_AMPDU)
1286 			info->flags |= IEEE80211_TX_CTL_DONTFRAG;
1287 	}
1288 
1289 	if (!tx->sta)
1290 		info->flags |= IEEE80211_TX_CTL_CLEAR_PS_FILT;
1291 	else if (test_and_clear_sta_flag(tx->sta, WLAN_STA_CLEAR_PS_FILT)) {
1292 		info->flags |= IEEE80211_TX_CTL_CLEAR_PS_FILT;
1293 		ieee80211_check_fast_xmit(tx->sta);
1294 	}
1295 
1296 	info->flags |= IEEE80211_TX_CTL_FIRST_FRAGMENT;
1297 
1298 	return TX_CONTINUE;
1299 }
1300 
ieee80211_get_txq(struct ieee80211_local * local,struct ieee80211_vif * vif,struct sta_info * sta,struct sk_buff * skb)1301 static struct txq_info *ieee80211_get_txq(struct ieee80211_local *local,
1302 					  struct ieee80211_vif *vif,
1303 					  struct sta_info *sta,
1304 					  struct sk_buff *skb)
1305 {
1306 	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
1307 	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
1308 	struct ieee80211_txq *txq = NULL;
1309 
1310 	if ((info->flags & IEEE80211_TX_CTL_SEND_AFTER_DTIM) ||
1311 	    (info->control.flags & IEEE80211_TX_CTRL_PS_RESPONSE))
1312 		return NULL;
1313 
1314 	if (!(info->flags & IEEE80211_TX_CTL_HW_80211_ENCAP) &&
1315 	    unlikely(!ieee80211_is_data_present(hdr->frame_control))) {
1316 		if ((!ieee80211_is_mgmt(hdr->frame_control) ||
1317 		     ieee80211_is_bufferable_mmpdu(skb) ||
1318 		     vif->type == NL80211_IFTYPE_STATION) &&
1319 		    sta && sta->uploaded) {
1320 			/*
1321 			 * This will be NULL if the driver didn't set the
1322 			 * opt-in hardware flag.
1323 			 */
1324 			txq = sta->sta.txq[IEEE80211_NUM_TIDS];
1325 		}
1326 	} else if (sta) {
1327 		u8 tid = skb->priority & IEEE80211_QOS_CTL_TID_MASK;
1328 
1329 		if (!sta->uploaded)
1330 			return NULL;
1331 
1332 		txq = sta->sta.txq[tid];
1333 	} else {
1334 		txq = vif->txq;
1335 	}
1336 
1337 	if (!txq)
1338 		return NULL;
1339 
1340 	return to_txq_info(txq);
1341 }
1342 
ieee80211_set_skb_enqueue_time(struct sk_buff * skb)1343 static void ieee80211_set_skb_enqueue_time(struct sk_buff *skb)
1344 {
1345 	struct sk_buff *next;
1346 	codel_time_t now = codel_get_time();
1347 
1348 	skb_list_walk_safe(skb, skb, next)
1349 		IEEE80211_SKB_CB(skb)->control.enqueue_time = now;
1350 }
1351 
codel_skb_len_func(const struct sk_buff * skb)1352 static u32 codel_skb_len_func(const struct sk_buff *skb)
1353 {
1354 	return skb->len;
1355 }
1356 
codel_skb_time_func(const struct sk_buff * skb)1357 static codel_time_t codel_skb_time_func(const struct sk_buff *skb)
1358 {
1359 	const struct ieee80211_tx_info *info;
1360 
1361 	info = (const struct ieee80211_tx_info *)skb->cb;
1362 	return info->control.enqueue_time;
1363 }
1364 
codel_dequeue_func(struct codel_vars * cvars,void * ctx)1365 static struct sk_buff *codel_dequeue_func(struct codel_vars *cvars,
1366 					  void *ctx)
1367 {
1368 	struct ieee80211_local *local;
1369 	struct txq_info *txqi;
1370 	struct fq *fq;
1371 	struct fq_flow *flow;
1372 
1373 	txqi = ctx;
1374 	local = vif_to_sdata(txqi->txq.vif)->local;
1375 	fq = &local->fq;
1376 
1377 	if (cvars == &txqi->def_cvars)
1378 		flow = &txqi->tin.default_flow;
1379 	else
1380 		flow = &fq->flows[cvars - local->cvars];
1381 
1382 	return fq_flow_dequeue(fq, flow);
1383 }
1384 
codel_drop_func(struct sk_buff * skb,void * ctx)1385 static void codel_drop_func(struct sk_buff *skb,
1386 			    void *ctx)
1387 {
1388 	struct ieee80211_local *local;
1389 	struct ieee80211_hw *hw;
1390 	struct txq_info *txqi;
1391 
1392 	txqi = ctx;
1393 	local = vif_to_sdata(txqi->txq.vif)->local;
1394 	hw = &local->hw;
1395 
1396 	ieee80211_free_txskb(hw, skb);
1397 }
1398 
fq_tin_dequeue_func(struct fq * fq,struct fq_tin * tin,struct fq_flow * flow)1399 static struct sk_buff *fq_tin_dequeue_func(struct fq *fq,
1400 					   struct fq_tin *tin,
1401 					   struct fq_flow *flow)
1402 {
1403 	struct ieee80211_local *local;
1404 	struct txq_info *txqi;
1405 	struct codel_vars *cvars;
1406 	struct codel_params *cparams;
1407 	struct codel_stats *cstats;
1408 
1409 	local = container_of(fq, struct ieee80211_local, fq);
1410 	txqi = container_of(tin, struct txq_info, tin);
1411 	cstats = &txqi->cstats;
1412 
1413 	if (txqi->txq.sta) {
1414 		struct sta_info *sta = container_of(txqi->txq.sta,
1415 						    struct sta_info, sta);
1416 		cparams = &sta->cparams;
1417 	} else {
1418 		cparams = &local->cparams;
1419 	}
1420 
1421 	if (flow == &tin->default_flow)
1422 		cvars = &txqi->def_cvars;
1423 	else
1424 		cvars = &local->cvars[flow - fq->flows];
1425 
1426 	return codel_dequeue(txqi,
1427 			     &flow->backlog,
1428 			     cparams,
1429 			     cvars,
1430 			     cstats,
1431 			     codel_skb_len_func,
1432 			     codel_skb_time_func,
1433 			     codel_drop_func,
1434 			     codel_dequeue_func);
1435 }
1436 
fq_skb_free_func(struct fq * fq,struct fq_tin * tin,struct fq_flow * flow,struct sk_buff * skb)1437 static void fq_skb_free_func(struct fq *fq,
1438 			     struct fq_tin *tin,
1439 			     struct fq_flow *flow,
1440 			     struct sk_buff *skb)
1441 {
1442 	struct ieee80211_local *local;
1443 
1444 	local = container_of(fq, struct ieee80211_local, fq);
1445 	ieee80211_free_txskb(&local->hw, skb);
1446 }
1447 
ieee80211_txq_enqueue(struct ieee80211_local * local,struct txq_info * txqi,struct sk_buff * skb)1448 static void ieee80211_txq_enqueue(struct ieee80211_local *local,
1449 				  struct txq_info *txqi,
1450 				  struct sk_buff *skb)
1451 {
1452 	struct fq *fq = &local->fq;
1453 	struct fq_tin *tin = &txqi->tin;
1454 	u32 flow_idx = fq_flow_idx(fq, skb);
1455 
1456 	ieee80211_set_skb_enqueue_time(skb);
1457 
1458 	spin_lock_bh(&fq->lock);
1459 	/*
1460 	 * For management frames, don't really apply codel etc.,
1461 	 * we don't want to apply any shaping or anything we just
1462 	 * want to simplify the driver API by having them on the
1463 	 * txqi.
1464 	 */
1465 	if (unlikely(txqi->txq.tid == IEEE80211_NUM_TIDS)) {
1466 		IEEE80211_SKB_CB(skb)->control.flags |=
1467 			IEEE80211_TX_INTCFL_NEED_TXPROCESSING;
1468 		__skb_queue_tail(&txqi->frags, skb);
1469 	} else {
1470 		fq_tin_enqueue(fq, tin, flow_idx, skb,
1471 			       fq_skb_free_func);
1472 	}
1473 	spin_unlock_bh(&fq->lock);
1474 }
1475 
fq_vlan_filter_func(struct fq * fq,struct fq_tin * tin,struct fq_flow * flow,struct sk_buff * skb,void * data)1476 static bool fq_vlan_filter_func(struct fq *fq, struct fq_tin *tin,
1477 				struct fq_flow *flow, struct sk_buff *skb,
1478 				void *data)
1479 {
1480 	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
1481 
1482 	return info->control.vif == data;
1483 }
1484 
ieee80211_txq_remove_vlan(struct ieee80211_local * local,struct ieee80211_sub_if_data * sdata)1485 void ieee80211_txq_remove_vlan(struct ieee80211_local *local,
1486 			       struct ieee80211_sub_if_data *sdata)
1487 {
1488 	struct fq *fq = &local->fq;
1489 	struct txq_info *txqi;
1490 	struct fq_tin *tin;
1491 	struct ieee80211_sub_if_data *ap;
1492 
1493 	if (WARN_ON(sdata->vif.type != NL80211_IFTYPE_AP_VLAN))
1494 		return;
1495 
1496 	ap = container_of(sdata->bss, struct ieee80211_sub_if_data, u.ap);
1497 
1498 	if (!ap->vif.txq)
1499 		return;
1500 
1501 	txqi = to_txq_info(ap->vif.txq);
1502 	tin = &txqi->tin;
1503 
1504 	spin_lock_bh(&fq->lock);
1505 	fq_tin_filter(fq, tin, fq_vlan_filter_func, &sdata->vif,
1506 		      fq_skb_free_func);
1507 	spin_unlock_bh(&fq->lock);
1508 }
1509 
ieee80211_txq_init(struct ieee80211_sub_if_data * sdata,struct sta_info * sta,struct txq_info * txqi,int tid)1510 void ieee80211_txq_init(struct ieee80211_sub_if_data *sdata,
1511 			struct sta_info *sta,
1512 			struct txq_info *txqi, int tid)
1513 {
1514 	fq_tin_init(&txqi->tin);
1515 	codel_vars_init(&txqi->def_cvars);
1516 	codel_stats_init(&txqi->cstats);
1517 	__skb_queue_head_init(&txqi->frags);
1518 	INIT_LIST_HEAD(&txqi->schedule_order);
1519 
1520 	txqi->txq.vif = &sdata->vif;
1521 
1522 	if (!sta) {
1523 		sdata->vif.txq = &txqi->txq;
1524 		txqi->txq.tid = 0;
1525 		txqi->txq.ac = IEEE80211_AC_BE;
1526 
1527 		return;
1528 	}
1529 
1530 	if (tid == IEEE80211_NUM_TIDS) {
1531 		if (sdata->vif.type == NL80211_IFTYPE_STATION) {
1532 			/* Drivers need to opt in to the management MPDU TXQ */
1533 			if (!ieee80211_hw_check(&sdata->local->hw,
1534 						STA_MMPDU_TXQ))
1535 				return;
1536 		} else if (!ieee80211_hw_check(&sdata->local->hw,
1537 					       BUFF_MMPDU_TXQ)) {
1538 			/* Drivers need to opt in to the bufferable MMPDU TXQ */
1539 			return;
1540 		}
1541 		txqi->txq.ac = IEEE80211_AC_VO;
1542 	} else {
1543 		txqi->txq.ac = ieee80211_ac_from_tid(tid);
1544 	}
1545 
1546 	txqi->txq.sta = &sta->sta;
1547 	txqi->txq.tid = tid;
1548 	sta->sta.txq[tid] = &txqi->txq;
1549 }
1550 
ieee80211_txq_purge(struct ieee80211_local * local,struct txq_info * txqi)1551 void ieee80211_txq_purge(struct ieee80211_local *local,
1552 			 struct txq_info *txqi)
1553 {
1554 	struct fq *fq = &local->fq;
1555 	struct fq_tin *tin = &txqi->tin;
1556 
1557 	spin_lock_bh(&fq->lock);
1558 	fq_tin_reset(fq, tin, fq_skb_free_func);
1559 	ieee80211_purge_tx_queue(&local->hw, &txqi->frags);
1560 	spin_unlock_bh(&fq->lock);
1561 
1562 	spin_lock_bh(&local->active_txq_lock[txqi->txq.ac]);
1563 	list_del_init(&txqi->schedule_order);
1564 	spin_unlock_bh(&local->active_txq_lock[txqi->txq.ac]);
1565 }
1566 
ieee80211_txq_set_params(struct ieee80211_local * local)1567 void ieee80211_txq_set_params(struct ieee80211_local *local)
1568 {
1569 	if (local->hw.wiphy->txq_limit)
1570 		local->fq.limit = local->hw.wiphy->txq_limit;
1571 	else
1572 		local->hw.wiphy->txq_limit = local->fq.limit;
1573 
1574 	if (local->hw.wiphy->txq_memory_limit)
1575 		local->fq.memory_limit = local->hw.wiphy->txq_memory_limit;
1576 	else
1577 		local->hw.wiphy->txq_memory_limit = local->fq.memory_limit;
1578 
1579 	if (local->hw.wiphy->txq_quantum)
1580 		local->fq.quantum = local->hw.wiphy->txq_quantum;
1581 	else
1582 		local->hw.wiphy->txq_quantum = local->fq.quantum;
1583 }
1584 
ieee80211_txq_setup_flows(struct ieee80211_local * local)1585 int ieee80211_txq_setup_flows(struct ieee80211_local *local)
1586 {
1587 	struct fq *fq = &local->fq;
1588 	int ret;
1589 	int i;
1590 	bool supp_vht = false;
1591 	enum nl80211_band band;
1592 
1593 	ret = fq_init(fq, 4096);
1594 	if (ret)
1595 		return ret;
1596 
1597 	/*
1598 	 * If the hardware doesn't support VHT, it is safe to limit the maximum
1599 	 * queue size. 4 Mbytes is 64 max-size aggregates in 802.11n.
1600 	 */
1601 	for (band = 0; band < NUM_NL80211_BANDS; band++) {
1602 		struct ieee80211_supported_band *sband;
1603 
1604 		sband = local->hw.wiphy->bands[band];
1605 		if (!sband)
1606 			continue;
1607 
1608 		supp_vht = supp_vht || sband->vht_cap.vht_supported;
1609 	}
1610 
1611 	if (!supp_vht)
1612 		fq->memory_limit = 4 << 20; /* 4 Mbytes */
1613 
1614 	codel_params_init(&local->cparams);
1615 	local->cparams.interval = MS2TIME(100);
1616 	local->cparams.target = MS2TIME(20);
1617 	local->cparams.ecn = true;
1618 
1619 	local->cvars = kcalloc(fq->flows_cnt, sizeof(local->cvars[0]),
1620 			       GFP_KERNEL);
1621 	if (!local->cvars) {
1622 		spin_lock_bh(&fq->lock);
1623 		fq_reset(fq, fq_skb_free_func);
1624 		spin_unlock_bh(&fq->lock);
1625 		return -ENOMEM;
1626 	}
1627 
1628 	for (i = 0; i < fq->flows_cnt; i++)
1629 		codel_vars_init(&local->cvars[i]);
1630 
1631 	ieee80211_txq_set_params(local);
1632 
1633 	return 0;
1634 }
1635 
ieee80211_txq_teardown_flows(struct ieee80211_local * local)1636 void ieee80211_txq_teardown_flows(struct ieee80211_local *local)
1637 {
1638 	struct fq *fq = &local->fq;
1639 
1640 	kfree(local->cvars);
1641 	local->cvars = NULL;
1642 
1643 	spin_lock_bh(&fq->lock);
1644 	fq_reset(fq, fq_skb_free_func);
1645 	spin_unlock_bh(&fq->lock);
1646 }
1647 
ieee80211_queue_skb(struct ieee80211_local * local,struct ieee80211_sub_if_data * sdata,struct sta_info * sta,struct sk_buff * skb)1648 static bool ieee80211_queue_skb(struct ieee80211_local *local,
1649 				struct ieee80211_sub_if_data *sdata,
1650 				struct sta_info *sta,
1651 				struct sk_buff *skb)
1652 {
1653 	struct ieee80211_vif *vif;
1654 	struct txq_info *txqi;
1655 
1656 	if (sdata->vif.type == NL80211_IFTYPE_MONITOR)
1657 		return false;
1658 
1659 	if (sdata->vif.type == NL80211_IFTYPE_AP_VLAN)
1660 		sdata = container_of(sdata->bss,
1661 				     struct ieee80211_sub_if_data, u.ap);
1662 
1663 	vif = &sdata->vif;
1664 	txqi = ieee80211_get_txq(local, vif, sta, skb);
1665 
1666 	if (!txqi)
1667 		return false;
1668 
1669 	ieee80211_txq_enqueue(local, txqi, skb);
1670 
1671 	schedule_and_wake_txq(local, txqi);
1672 
1673 	return true;
1674 }
1675 
ieee80211_tx_frags(struct ieee80211_local * local,struct ieee80211_vif * vif,struct sta_info * sta,struct sk_buff_head * skbs,bool txpending)1676 static bool ieee80211_tx_frags(struct ieee80211_local *local,
1677 			       struct ieee80211_vif *vif,
1678 			       struct sta_info *sta,
1679 			       struct sk_buff_head *skbs,
1680 			       bool txpending)
1681 {
1682 	struct ieee80211_tx_control control = {};
1683 	struct sk_buff *skb, *tmp;
1684 	unsigned long flags;
1685 
1686 	skb_queue_walk_safe(skbs, skb, tmp) {
1687 		struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
1688 		int q = info->hw_queue;
1689 
1690 #ifdef CONFIG_MAC80211_VERBOSE_DEBUG
1691 		if (WARN_ON_ONCE(q >= local->hw.queues)) {
1692 			__skb_unlink(skb, skbs);
1693 			ieee80211_free_txskb(&local->hw, skb);
1694 			continue;
1695 		}
1696 #endif
1697 
1698 		spin_lock_irqsave(&local->queue_stop_reason_lock, flags);
1699 		if (local->queue_stop_reasons[q] ||
1700 		    (!txpending && !skb_queue_empty(&local->pending[q]))) {
1701 			if (unlikely(info->flags &
1702 				     IEEE80211_TX_INTFL_OFFCHAN_TX_OK)) {
1703 				if (local->queue_stop_reasons[q] &
1704 				    ~BIT(IEEE80211_QUEUE_STOP_REASON_OFFCHANNEL)) {
1705 					/*
1706 					 * Drop off-channel frames if queues
1707 					 * are stopped for any reason other
1708 					 * than off-channel operation. Never
1709 					 * queue them.
1710 					 */
1711 					spin_unlock_irqrestore(
1712 						&local->queue_stop_reason_lock,
1713 						flags);
1714 					ieee80211_purge_tx_queue(&local->hw,
1715 								 skbs);
1716 					return true;
1717 				}
1718 			} else {
1719 
1720 				/*
1721 				 * Since queue is stopped, queue up frames for
1722 				 * later transmission from the tx-pending
1723 				 * tasklet when the queue is woken again.
1724 				 */
1725 				if (txpending)
1726 					skb_queue_splice_init(skbs,
1727 							      &local->pending[q]);
1728 				else
1729 					skb_queue_splice_tail_init(skbs,
1730 								   &local->pending[q]);
1731 
1732 				spin_unlock_irqrestore(&local->queue_stop_reason_lock,
1733 						       flags);
1734 				return false;
1735 			}
1736 		}
1737 		spin_unlock_irqrestore(&local->queue_stop_reason_lock, flags);
1738 
1739 		info->control.vif = vif;
1740 		control.sta = sta ? &sta->sta : NULL;
1741 
1742 		__skb_unlink(skb, skbs);
1743 		drv_tx(local, &control, skb);
1744 	}
1745 
1746 	return true;
1747 }
1748 
1749 /*
1750  * Returns false if the frame couldn't be transmitted but was queued instead.
1751  */
__ieee80211_tx(struct ieee80211_local * local,struct sk_buff_head * skbs,struct sta_info * sta,bool txpending)1752 static bool __ieee80211_tx(struct ieee80211_local *local,
1753 			   struct sk_buff_head *skbs, struct sta_info *sta,
1754 			   bool txpending)
1755 {
1756 	struct ieee80211_tx_info *info;
1757 	struct ieee80211_sub_if_data *sdata;
1758 	struct ieee80211_vif *vif;
1759 	struct sk_buff *skb;
1760 	bool result;
1761 
1762 	if (WARN_ON(skb_queue_empty(skbs)))
1763 		return true;
1764 
1765 	skb = skb_peek(skbs);
1766 	info = IEEE80211_SKB_CB(skb);
1767 	sdata = vif_to_sdata(info->control.vif);
1768 	if (sta && !sta->uploaded)
1769 		sta = NULL;
1770 
1771 	switch (sdata->vif.type) {
1772 	case NL80211_IFTYPE_MONITOR:
1773 		if (sdata->u.mntr.flags & MONITOR_FLAG_ACTIVE) {
1774 			vif = &sdata->vif;
1775 			break;
1776 		}
1777 		sdata = rcu_dereference(local->monitor_sdata);
1778 		if (sdata) {
1779 			vif = &sdata->vif;
1780 			info->hw_queue =
1781 				vif->hw_queue[skb_get_queue_mapping(skb)];
1782 		} else if (ieee80211_hw_check(&local->hw, QUEUE_CONTROL)) {
1783 			ieee80211_purge_tx_queue(&local->hw, skbs);
1784 			return true;
1785 		} else
1786 			vif = NULL;
1787 		break;
1788 	case NL80211_IFTYPE_AP_VLAN:
1789 		sdata = container_of(sdata->bss,
1790 				     struct ieee80211_sub_if_data, u.ap);
1791 		fallthrough;
1792 	default:
1793 		vif = &sdata->vif;
1794 		break;
1795 	}
1796 
1797 	result = ieee80211_tx_frags(local, vif, sta, skbs, txpending);
1798 
1799 	WARN_ON_ONCE(!skb_queue_empty(skbs));
1800 
1801 	return result;
1802 }
1803 
1804 /*
1805  * Invoke TX handlers, return 0 on success and non-zero if the
1806  * frame was dropped or queued.
1807  *
1808  * The handlers are split into an early and late part. The latter is everything
1809  * that can be sensitive to reordering, and will be deferred to after packets
1810  * are dequeued from the intermediate queues (when they are enabled).
1811  */
invoke_tx_handlers_early(struct ieee80211_tx_data * tx)1812 static int invoke_tx_handlers_early(struct ieee80211_tx_data *tx)
1813 {
1814 	ieee80211_tx_result res = TX_DROP;
1815 
1816 #define CALL_TXH(txh) \
1817 	do {				\
1818 		res = txh(tx);		\
1819 		if (res != TX_CONTINUE)	\
1820 			goto txh_done;	\
1821 	} while (0)
1822 
1823 	CALL_TXH(ieee80211_tx_h_dynamic_ps);
1824 	CALL_TXH(ieee80211_tx_h_check_assoc);
1825 	CALL_TXH(ieee80211_tx_h_ps_buf);
1826 	CALL_TXH(ieee80211_tx_h_check_control_port_protocol);
1827 	CALL_TXH(ieee80211_tx_h_select_key);
1828 
1829  txh_done:
1830 	if (unlikely(res == TX_DROP)) {
1831 		I802_DEBUG_INC(tx->local->tx_handlers_drop);
1832 		if (tx->skb)
1833 			ieee80211_free_txskb(&tx->local->hw, tx->skb);
1834 		else
1835 			ieee80211_purge_tx_queue(&tx->local->hw, &tx->skbs);
1836 		return -1;
1837 	} else if (unlikely(res == TX_QUEUED)) {
1838 		I802_DEBUG_INC(tx->local->tx_handlers_queued);
1839 		return -1;
1840 	}
1841 
1842 	return 0;
1843 }
1844 
1845 /*
1846  * Late handlers can be called while the sta lock is held. Handlers that can
1847  * cause packets to be generated will cause deadlock!
1848  */
invoke_tx_handlers_late(struct ieee80211_tx_data * tx)1849 static int invoke_tx_handlers_late(struct ieee80211_tx_data *tx)
1850 {
1851 	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(tx->skb);
1852 	ieee80211_tx_result res = TX_CONTINUE;
1853 
1854 	if (!ieee80211_hw_check(&tx->local->hw, HAS_RATE_CONTROL))
1855 		CALL_TXH(ieee80211_tx_h_rate_ctrl);
1856 
1857 	if (unlikely(info->flags & IEEE80211_TX_INTFL_RETRANSMISSION)) {
1858 		__skb_queue_tail(&tx->skbs, tx->skb);
1859 		tx->skb = NULL;
1860 		goto txh_done;
1861 	}
1862 
1863 	CALL_TXH(ieee80211_tx_h_michael_mic_add);
1864 	CALL_TXH(ieee80211_tx_h_sequence);
1865 	CALL_TXH(ieee80211_tx_h_fragment);
1866 	/* handlers after fragment must be aware of tx info fragmentation! */
1867 	CALL_TXH(ieee80211_tx_h_stats);
1868 	CALL_TXH(ieee80211_tx_h_encrypt);
1869 	if (!ieee80211_hw_check(&tx->local->hw, HAS_RATE_CONTROL))
1870 		CALL_TXH(ieee80211_tx_h_calculate_duration);
1871 #undef CALL_TXH
1872 
1873  txh_done:
1874 	if (unlikely(res == TX_DROP)) {
1875 		I802_DEBUG_INC(tx->local->tx_handlers_drop);
1876 		if (tx->skb)
1877 			ieee80211_free_txskb(&tx->local->hw, tx->skb);
1878 		else
1879 			ieee80211_purge_tx_queue(&tx->local->hw, &tx->skbs);
1880 		return -1;
1881 	} else if (unlikely(res == TX_QUEUED)) {
1882 		I802_DEBUG_INC(tx->local->tx_handlers_queued);
1883 		return -1;
1884 	}
1885 
1886 	return 0;
1887 }
1888 
invoke_tx_handlers(struct ieee80211_tx_data * tx)1889 static int invoke_tx_handlers(struct ieee80211_tx_data *tx)
1890 {
1891 	int r = invoke_tx_handlers_early(tx);
1892 
1893 	if (r)
1894 		return r;
1895 	return invoke_tx_handlers_late(tx);
1896 }
1897 
ieee80211_tx_prepare_skb(struct ieee80211_hw * hw,struct ieee80211_vif * vif,struct sk_buff * skb,int band,struct ieee80211_sta ** sta)1898 bool ieee80211_tx_prepare_skb(struct ieee80211_hw *hw,
1899 			      struct ieee80211_vif *vif, struct sk_buff *skb,
1900 			      int band, struct ieee80211_sta **sta)
1901 {
1902 	struct ieee80211_sub_if_data *sdata = vif_to_sdata(vif);
1903 	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
1904 	struct ieee80211_tx_data tx;
1905 	struct sk_buff *skb2;
1906 
1907 	if (ieee80211_tx_prepare(sdata, &tx, NULL, skb) == TX_DROP)
1908 		return false;
1909 
1910 	info->band = band;
1911 	info->control.vif = vif;
1912 	info->hw_queue = vif->hw_queue[skb_get_queue_mapping(skb)];
1913 
1914 	if (invoke_tx_handlers(&tx))
1915 		return false;
1916 
1917 	if (sta) {
1918 		if (tx.sta)
1919 			*sta = &tx.sta->sta;
1920 		else
1921 			*sta = NULL;
1922 	}
1923 
1924 	/* this function isn't suitable for fragmented data frames */
1925 	skb2 = __skb_dequeue(&tx.skbs);
1926 	if (WARN_ON(skb2 != skb || !skb_queue_empty(&tx.skbs))) {
1927 		ieee80211_free_txskb(hw, skb2);
1928 		ieee80211_purge_tx_queue(hw, &tx.skbs);
1929 		return false;
1930 	}
1931 
1932 	return true;
1933 }
1934 EXPORT_SYMBOL(ieee80211_tx_prepare_skb);
1935 
1936 /*
1937  * Returns false if the frame couldn't be transmitted but was queued instead.
1938  */
ieee80211_tx(struct ieee80211_sub_if_data * sdata,struct sta_info * sta,struct sk_buff * skb,bool txpending)1939 static bool ieee80211_tx(struct ieee80211_sub_if_data *sdata,
1940 			 struct sta_info *sta, struct sk_buff *skb,
1941 			 bool txpending)
1942 {
1943 	struct ieee80211_local *local = sdata->local;
1944 	struct ieee80211_tx_data tx;
1945 	ieee80211_tx_result res_prepare;
1946 	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
1947 	bool result = true;
1948 
1949 	if (unlikely(skb->len < 10)) {
1950 		dev_kfree_skb(skb);
1951 		return true;
1952 	}
1953 
1954 	/* initialises tx */
1955 	res_prepare = ieee80211_tx_prepare(sdata, &tx, sta, skb);
1956 
1957 	if (unlikely(res_prepare == TX_DROP)) {
1958 		ieee80211_free_txskb(&local->hw, skb);
1959 		return true;
1960 	} else if (unlikely(res_prepare == TX_QUEUED)) {
1961 		return true;
1962 	}
1963 
1964 	/* set up hw_queue value early */
1965 	if (!(info->flags & IEEE80211_TX_CTL_TX_OFFCHAN) ||
1966 	    !ieee80211_hw_check(&local->hw, QUEUE_CONTROL))
1967 		info->hw_queue =
1968 			sdata->vif.hw_queue[skb_get_queue_mapping(skb)];
1969 
1970 	if (invoke_tx_handlers_early(&tx))
1971 		return true;
1972 
1973 	if (ieee80211_queue_skb(local, sdata, tx.sta, tx.skb))
1974 		return true;
1975 
1976 	if (!invoke_tx_handlers_late(&tx))
1977 		result = __ieee80211_tx(local, &tx.skbs, tx.sta, txpending);
1978 
1979 	return result;
1980 }
1981 
1982 /* device xmit handlers */
1983 
1984 enum ieee80211_encrypt {
1985 	ENCRYPT_NO,
1986 	ENCRYPT_MGMT,
1987 	ENCRYPT_DATA,
1988 };
1989 
ieee80211_skb_resize(struct ieee80211_sub_if_data * sdata,struct sk_buff * skb,int head_need,enum ieee80211_encrypt encrypt)1990 static int ieee80211_skb_resize(struct ieee80211_sub_if_data *sdata,
1991 				struct sk_buff *skb,
1992 				int head_need,
1993 				enum ieee80211_encrypt encrypt)
1994 {
1995 	struct ieee80211_local *local = sdata->local;
1996 	bool enc_tailroom;
1997 	int tail_need = 0;
1998 
1999 	enc_tailroom = encrypt == ENCRYPT_MGMT ||
2000 		       (encrypt == ENCRYPT_DATA &&
2001 			sdata->crypto_tx_tailroom_needed_cnt);
2002 
2003 	if (enc_tailroom) {
2004 		tail_need = IEEE80211_ENCRYPT_TAILROOM;
2005 		tail_need -= skb_tailroom(skb);
2006 		tail_need = max_t(int, tail_need, 0);
2007 	}
2008 
2009 	if (skb_cloned(skb) &&
2010 	    (!ieee80211_hw_check(&local->hw, SUPPORTS_CLONED_SKBS) ||
2011 	     !skb_clone_writable(skb, ETH_HLEN) || enc_tailroom))
2012 		I802_DEBUG_INC(local->tx_expand_skb_head_cloned);
2013 	else if (head_need || tail_need)
2014 		I802_DEBUG_INC(local->tx_expand_skb_head);
2015 	else
2016 		return 0;
2017 
2018 	if (pskb_expand_head(skb, head_need, tail_need, GFP_ATOMIC)) {
2019 		wiphy_debug(local->hw.wiphy,
2020 			    "failed to reallocate TX buffer\n");
2021 		return -ENOMEM;
2022 	}
2023 
2024 	return 0;
2025 }
2026 
ieee80211_xmit(struct ieee80211_sub_if_data * sdata,struct sta_info * sta,struct sk_buff * skb)2027 void ieee80211_xmit(struct ieee80211_sub_if_data *sdata,
2028 		    struct sta_info *sta, struct sk_buff *skb)
2029 {
2030 	struct ieee80211_local *local = sdata->local;
2031 	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
2032 	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
2033 	int headroom;
2034 	enum ieee80211_encrypt encrypt;
2035 
2036 	if (info->flags & IEEE80211_TX_INTFL_DONT_ENCRYPT)
2037 		encrypt = ENCRYPT_NO;
2038 	else if (ieee80211_is_mgmt(hdr->frame_control))
2039 		encrypt = ENCRYPT_MGMT;
2040 	else
2041 		encrypt = ENCRYPT_DATA;
2042 
2043 	headroom = local->tx_headroom;
2044 	if (encrypt != ENCRYPT_NO)
2045 		headroom += IEEE80211_ENCRYPT_HEADROOM;
2046 	headroom -= skb_headroom(skb);
2047 	headroom = max_t(int, 0, headroom);
2048 
2049 	if (ieee80211_skb_resize(sdata, skb, headroom, encrypt)) {
2050 		ieee80211_free_txskb(&local->hw, skb);
2051 		return;
2052 	}
2053 
2054 	/* reload after potential resize */
2055 	hdr = (struct ieee80211_hdr *) skb->data;
2056 	info->control.vif = &sdata->vif;
2057 
2058 	if (ieee80211_vif_is_mesh(&sdata->vif)) {
2059 		if (ieee80211_is_data(hdr->frame_control) &&
2060 		    is_unicast_ether_addr(hdr->addr1)) {
2061 			if (mesh_nexthop_resolve(sdata, skb))
2062 				return; /* skb queued: don't free */
2063 		} else {
2064 			ieee80211_mps_set_frame_flags(sdata, NULL, hdr);
2065 		}
2066 	}
2067 
2068 	ieee80211_set_qos_hdr(sdata, skb);
2069 	ieee80211_tx(sdata, sta, skb, false);
2070 }
2071 
ieee80211_validate_radiotap_len(struct sk_buff * skb)2072 static bool ieee80211_validate_radiotap_len(struct sk_buff *skb)
2073 {
2074 	struct ieee80211_radiotap_header *rthdr =
2075 		(struct ieee80211_radiotap_header *)skb->data;
2076 
2077 	/* check for not even having the fixed radiotap header part */
2078 	if (unlikely(skb->len < sizeof(struct ieee80211_radiotap_header)))
2079 		return false; /* too short to be possibly valid */
2080 
2081 	/* is it a header version we can trust to find length from? */
2082 	if (unlikely(rthdr->it_version))
2083 		return false; /* only version 0 is supported */
2084 
2085 	/* does the skb contain enough to deliver on the alleged length? */
2086 	if (unlikely(skb->len < ieee80211_get_radiotap_len(skb->data)))
2087 		return false; /* skb too short for claimed rt header extent */
2088 
2089 	return true;
2090 }
2091 
ieee80211_parse_tx_radiotap(struct sk_buff * skb,struct net_device * dev)2092 bool ieee80211_parse_tx_radiotap(struct sk_buff *skb,
2093 				 struct net_device *dev)
2094 {
2095 	struct ieee80211_local *local = wdev_priv(dev->ieee80211_ptr);
2096 	struct ieee80211_radiotap_iterator iterator;
2097 	struct ieee80211_radiotap_header *rthdr =
2098 		(struct ieee80211_radiotap_header *) skb->data;
2099 	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
2100 	int ret = ieee80211_radiotap_iterator_init(&iterator, rthdr, skb->len,
2101 						   NULL);
2102 	u16 txflags;
2103 	u16 rate = 0;
2104 	bool rate_found = false;
2105 	u8 rate_retries = 0;
2106 	u16 rate_flags = 0;
2107 	u8 mcs_known, mcs_flags, mcs_bw;
2108 	u16 vht_known;
2109 	u8 vht_mcs = 0, vht_nss = 0;
2110 	int i;
2111 
2112 	if (!ieee80211_validate_radiotap_len(skb))
2113 		return false;
2114 
2115 	info->flags |= IEEE80211_TX_INTFL_DONT_ENCRYPT |
2116 		       IEEE80211_TX_CTL_DONTFRAG;
2117 
2118 	/*
2119 	 * for every radiotap entry that is present
2120 	 * (ieee80211_radiotap_iterator_next returns -ENOENT when no more
2121 	 * entries present, or -EINVAL on error)
2122 	 */
2123 
2124 	while (!ret) {
2125 		ret = ieee80211_radiotap_iterator_next(&iterator);
2126 
2127 		if (ret)
2128 			continue;
2129 
2130 		/* see if this argument is something we can use */
2131 		switch (iterator.this_arg_index) {
2132 		/*
2133 		 * You must take care when dereferencing iterator.this_arg
2134 		 * for multibyte types... the pointer is not aligned.  Use
2135 		 * get_unaligned((type *)iterator.this_arg) to dereference
2136 		 * iterator.this_arg for type "type" safely on all arches.
2137 		*/
2138 		case IEEE80211_RADIOTAP_FLAGS:
2139 			if (*iterator.this_arg & IEEE80211_RADIOTAP_F_FCS) {
2140 				/*
2141 				 * this indicates that the skb we have been
2142 				 * handed has the 32-bit FCS CRC at the end...
2143 				 * we should react to that by snipping it off
2144 				 * because it will be recomputed and added
2145 				 * on transmission
2146 				 */
2147 				if (skb->len < (iterator._max_length + FCS_LEN))
2148 					return false;
2149 
2150 				skb_trim(skb, skb->len - FCS_LEN);
2151 			}
2152 			if (*iterator.this_arg & IEEE80211_RADIOTAP_F_WEP)
2153 				info->flags &= ~IEEE80211_TX_INTFL_DONT_ENCRYPT;
2154 			if (*iterator.this_arg & IEEE80211_RADIOTAP_F_FRAG)
2155 				info->flags &= ~IEEE80211_TX_CTL_DONTFRAG;
2156 			break;
2157 
2158 		case IEEE80211_RADIOTAP_TX_FLAGS:
2159 			txflags = get_unaligned_le16(iterator.this_arg);
2160 			if (txflags & IEEE80211_RADIOTAP_F_TX_NOACK)
2161 				info->flags |= IEEE80211_TX_CTL_NO_ACK;
2162 			if (txflags & IEEE80211_RADIOTAP_F_TX_NOSEQNO)
2163 				info->control.flags |= IEEE80211_TX_CTRL_NO_SEQNO;
2164 			if (txflags & IEEE80211_RADIOTAP_F_TX_ORDER)
2165 				info->control.flags |=
2166 					IEEE80211_TX_CTRL_DONT_REORDER;
2167 			break;
2168 
2169 		case IEEE80211_RADIOTAP_RATE:
2170 			rate = *iterator.this_arg;
2171 			rate_flags = 0;
2172 			rate_found = true;
2173 			break;
2174 
2175 		case IEEE80211_RADIOTAP_DATA_RETRIES:
2176 			rate_retries = *iterator.this_arg;
2177 			break;
2178 
2179 		case IEEE80211_RADIOTAP_MCS:
2180 			mcs_known = iterator.this_arg[0];
2181 			mcs_flags = iterator.this_arg[1];
2182 			if (!(mcs_known & IEEE80211_RADIOTAP_MCS_HAVE_MCS))
2183 				break;
2184 
2185 			rate_found = true;
2186 			rate = iterator.this_arg[2];
2187 			rate_flags = IEEE80211_TX_RC_MCS;
2188 
2189 			if (mcs_known & IEEE80211_RADIOTAP_MCS_HAVE_GI &&
2190 			    mcs_flags & IEEE80211_RADIOTAP_MCS_SGI)
2191 				rate_flags |= IEEE80211_TX_RC_SHORT_GI;
2192 
2193 			mcs_bw = mcs_flags & IEEE80211_RADIOTAP_MCS_BW_MASK;
2194 			if (mcs_known & IEEE80211_RADIOTAP_MCS_HAVE_BW &&
2195 			    mcs_bw == IEEE80211_RADIOTAP_MCS_BW_40)
2196 				rate_flags |= IEEE80211_TX_RC_40_MHZ_WIDTH;
2197 
2198 			if (mcs_known & IEEE80211_RADIOTAP_MCS_HAVE_FEC &&
2199 			    mcs_flags & IEEE80211_RADIOTAP_MCS_FEC_LDPC)
2200 				info->flags |= IEEE80211_TX_CTL_LDPC;
2201 
2202 			if (mcs_known & IEEE80211_RADIOTAP_MCS_HAVE_STBC) {
2203 				u8 stbc = u8_get_bits(mcs_flags,
2204 						      IEEE80211_RADIOTAP_MCS_STBC_MASK);
2205 
2206 				info->flags |=
2207 					u32_encode_bits(stbc,
2208 							IEEE80211_TX_CTL_STBC);
2209 			}
2210 			break;
2211 
2212 		case IEEE80211_RADIOTAP_VHT:
2213 			vht_known = get_unaligned_le16(iterator.this_arg);
2214 			rate_found = true;
2215 
2216 			rate_flags = IEEE80211_TX_RC_VHT_MCS;
2217 			if ((vht_known & IEEE80211_RADIOTAP_VHT_KNOWN_GI) &&
2218 			    (iterator.this_arg[2] &
2219 			     IEEE80211_RADIOTAP_VHT_FLAG_SGI))
2220 				rate_flags |= IEEE80211_TX_RC_SHORT_GI;
2221 			if (vht_known &
2222 			    IEEE80211_RADIOTAP_VHT_KNOWN_BANDWIDTH) {
2223 				if (iterator.this_arg[3] == 1)
2224 					rate_flags |=
2225 						IEEE80211_TX_RC_40_MHZ_WIDTH;
2226 				else if (iterator.this_arg[3] == 4)
2227 					rate_flags |=
2228 						IEEE80211_TX_RC_80_MHZ_WIDTH;
2229 				else if (iterator.this_arg[3] == 11)
2230 					rate_flags |=
2231 						IEEE80211_TX_RC_160_MHZ_WIDTH;
2232 			}
2233 
2234 			vht_mcs = iterator.this_arg[4] >> 4;
2235 			if (vht_mcs > 11)
2236 				vht_mcs = 0;
2237 			vht_nss = iterator.this_arg[4] & 0xF;
2238 			if (!vht_nss || vht_nss > 8)
2239 				vht_nss = 1;
2240 			break;
2241 
2242 		/*
2243 		 * Please update the file
2244 		 * Documentation/networking/mac80211-injection.rst
2245 		 * when parsing new fields here.
2246 		 */
2247 
2248 		default:
2249 			break;
2250 		}
2251 	}
2252 
2253 	if (ret != -ENOENT) /* ie, if we didn't simply run out of fields */
2254 		return false;
2255 
2256 	if (rate_found) {
2257 		struct ieee80211_supported_band *sband =
2258 			local->hw.wiphy->bands[info->band];
2259 
2260 		info->control.flags |= IEEE80211_TX_CTRL_RATE_INJECT;
2261 
2262 		for (i = 0; i < IEEE80211_TX_MAX_RATES; i++) {
2263 			info->control.rates[i].idx = -1;
2264 			info->control.rates[i].flags = 0;
2265 			info->control.rates[i].count = 0;
2266 		}
2267 
2268 		if (rate_flags & IEEE80211_TX_RC_MCS) {
2269 			info->control.rates[0].idx = rate;
2270 		} else if (rate_flags & IEEE80211_TX_RC_VHT_MCS) {
2271 			ieee80211_rate_set_vht(info->control.rates, vht_mcs,
2272 					       vht_nss);
2273 		} else if (sband) {
2274 			for (i = 0; i < sband->n_bitrates; i++) {
2275 				if (rate * 5 != sband->bitrates[i].bitrate)
2276 					continue;
2277 
2278 				info->control.rates[0].idx = i;
2279 				break;
2280 			}
2281 		}
2282 
2283 		if (info->control.rates[0].idx < 0)
2284 			info->control.flags &= ~IEEE80211_TX_CTRL_RATE_INJECT;
2285 
2286 		info->control.rates[0].flags = rate_flags;
2287 		info->control.rates[0].count = min_t(u8, rate_retries + 1,
2288 						     local->hw.max_rate_tries);
2289 	}
2290 
2291 	return true;
2292 }
2293 
ieee80211_monitor_start_xmit(struct sk_buff * skb,struct net_device * dev)2294 netdev_tx_t ieee80211_monitor_start_xmit(struct sk_buff *skb,
2295 					 struct net_device *dev)
2296 {
2297 	struct ieee80211_local *local = wdev_priv(dev->ieee80211_ptr);
2298 	struct ieee80211_chanctx_conf *chanctx_conf;
2299 	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
2300 	struct ieee80211_hdr *hdr;
2301 	struct ieee80211_sub_if_data *tmp_sdata, *sdata;
2302 	struct cfg80211_chan_def *chandef;
2303 	u16 len_rthdr;
2304 	int hdrlen;
2305 
2306 	sdata = IEEE80211_DEV_TO_SUB_IF(dev);
2307 	if (unlikely(!ieee80211_sdata_running(sdata)))
2308 		goto fail;
2309 
2310 	memset(info, 0, sizeof(*info));
2311 	info->flags = IEEE80211_TX_CTL_REQ_TX_STATUS |
2312 		      IEEE80211_TX_CTL_INJECTED;
2313 
2314 	/* Sanity-check the length of the radiotap header */
2315 	if (!ieee80211_validate_radiotap_len(skb))
2316 		goto fail;
2317 
2318 	/* we now know there is a radiotap header with a length we can use */
2319 	len_rthdr = ieee80211_get_radiotap_len(skb->data);
2320 
2321 	/*
2322 	 * fix up the pointers accounting for the radiotap
2323 	 * header still being in there.  We are being given
2324 	 * a precooked IEEE80211 header so no need for
2325 	 * normal processing
2326 	 */
2327 	skb_set_mac_header(skb, len_rthdr);
2328 	/*
2329 	 * these are just fixed to the end of the rt area since we
2330 	 * don't have any better information and at this point, nobody cares
2331 	 */
2332 	skb_set_network_header(skb, len_rthdr);
2333 	skb_set_transport_header(skb, len_rthdr);
2334 
2335 	if (skb->len < len_rthdr + 2)
2336 		goto fail;
2337 
2338 	hdr = (struct ieee80211_hdr *)(skb->data + len_rthdr);
2339 	hdrlen = ieee80211_hdrlen(hdr->frame_control);
2340 
2341 	if (skb->len < len_rthdr + hdrlen)
2342 		goto fail;
2343 
2344 	/*
2345 	 * Initialize skb->protocol if the injected frame is a data frame
2346 	 * carrying a rfc1042 header
2347 	 */
2348 	if (ieee80211_is_data(hdr->frame_control) &&
2349 	    skb->len >= len_rthdr + hdrlen + sizeof(rfc1042_header) + 2) {
2350 		u8 *payload = (u8 *)hdr + hdrlen;
2351 
2352 		if (ether_addr_equal(payload, rfc1042_header))
2353 			skb->protocol = cpu_to_be16((payload[6] << 8) |
2354 						    payload[7]);
2355 	}
2356 
2357 	rcu_read_lock();
2358 
2359 	/*
2360 	 * We process outgoing injected frames that have a local address
2361 	 * we handle as though they are non-injected frames.
2362 	 * This code here isn't entirely correct, the local MAC address
2363 	 * isn't always enough to find the interface to use; for proper
2364 	 * VLAN support we have an nl80211-based mechanism.
2365 	 *
2366 	 * This is necessary, for example, for old hostapd versions that
2367 	 * don't use nl80211-based management TX/RX.
2368 	 */
2369 	list_for_each_entry_rcu(tmp_sdata, &local->interfaces, list) {
2370 		if (!ieee80211_sdata_running(tmp_sdata))
2371 			continue;
2372 		if (tmp_sdata->vif.type == NL80211_IFTYPE_MONITOR ||
2373 		    tmp_sdata->vif.type == NL80211_IFTYPE_AP_VLAN)
2374 			continue;
2375 		if (ether_addr_equal(tmp_sdata->vif.addr, hdr->addr2)) {
2376 			sdata = tmp_sdata;
2377 			break;
2378 		}
2379 	}
2380 
2381 	chanctx_conf = rcu_dereference(sdata->vif.bss_conf.chanctx_conf);
2382 	if (!chanctx_conf) {
2383 		tmp_sdata = rcu_dereference(local->monitor_sdata);
2384 		if (tmp_sdata)
2385 			chanctx_conf =
2386 				rcu_dereference(tmp_sdata->vif.bss_conf.chanctx_conf);
2387 	}
2388 
2389 	if (chanctx_conf)
2390 		chandef = &chanctx_conf->def;
2391 	else if (!local->use_chanctx)
2392 		chandef = &local->_oper_chandef;
2393 	else
2394 		goto fail_rcu;
2395 
2396 	/*
2397 	 * Frame injection is not allowed if beaconing is not allowed
2398 	 * or if we need radar detection. Beaconing is usually not allowed when
2399 	 * the mode or operation (Adhoc, AP, Mesh) does not support DFS.
2400 	 * Passive scan is also used in world regulatory domains where
2401 	 * your country is not known and as such it should be treated as
2402 	 * NO TX unless the channel is explicitly allowed in which case
2403 	 * your current regulatory domain would not have the passive scan
2404 	 * flag.
2405 	 *
2406 	 * Since AP mode uses monitor interfaces to inject/TX management
2407 	 * frames we can make AP mode the exception to this rule once it
2408 	 * supports radar detection as its implementation can deal with
2409 	 * radar detection by itself. We can do that later by adding a
2410 	 * monitor flag interfaces used for AP support.
2411 	 */
2412 	if (!cfg80211_reg_can_beacon(local->hw.wiphy, chandef,
2413 				     sdata->vif.type))
2414 		goto fail_rcu;
2415 
2416 	info->band = chandef->chan->band;
2417 
2418 	/* Initialize skb->priority according to frame type and TID class,
2419 	 * with respect to the sub interface that the frame will actually
2420 	 * be transmitted on. If the DONT_REORDER flag is set, the original
2421 	 * skb-priority is preserved to assure frames injected with this
2422 	 * flag are not reordered relative to each other.
2423 	 */
2424 	ieee80211_select_queue_80211(sdata, skb, hdr);
2425 	skb_set_queue_mapping(skb, ieee80211_ac_from_tid(skb->priority));
2426 
2427 	/*
2428 	 * Process the radiotap header. This will now take into account the
2429 	 * selected chandef above to accurately set injection rates and
2430 	 * retransmissions.
2431 	 */
2432 	if (!ieee80211_parse_tx_radiotap(skb, dev))
2433 		goto fail_rcu;
2434 
2435 	/* remove the injection radiotap header */
2436 	skb_pull(skb, len_rthdr);
2437 
2438 	ieee80211_xmit(sdata, NULL, skb);
2439 	rcu_read_unlock();
2440 
2441 	return NETDEV_TX_OK;
2442 
2443 fail_rcu:
2444 	rcu_read_unlock();
2445 fail:
2446 	dev_kfree_skb(skb);
2447 	return NETDEV_TX_OK; /* meaning, we dealt with the skb */
2448 }
2449 
ieee80211_is_tdls_setup(struct sk_buff * skb)2450 static inline bool ieee80211_is_tdls_setup(struct sk_buff *skb)
2451 {
2452 	u16 ethertype = (skb->data[12] << 8) | skb->data[13];
2453 
2454 	return ethertype == ETH_P_TDLS &&
2455 	       skb->len > 14 &&
2456 	       skb->data[14] == WLAN_TDLS_SNAP_RFTYPE;
2457 }
2458 
ieee80211_lookup_ra_sta(struct ieee80211_sub_if_data * sdata,struct sk_buff * skb,struct sta_info ** sta_out)2459 int ieee80211_lookup_ra_sta(struct ieee80211_sub_if_data *sdata,
2460 			    struct sk_buff *skb,
2461 			    struct sta_info **sta_out)
2462 {
2463 	struct sta_info *sta;
2464 
2465 	switch (sdata->vif.type) {
2466 	case NL80211_IFTYPE_AP_VLAN:
2467 		sta = rcu_dereference(sdata->u.vlan.sta);
2468 		if (sta) {
2469 			*sta_out = sta;
2470 			return 0;
2471 		} else if (sdata->wdev.use_4addr) {
2472 			return -ENOLINK;
2473 		}
2474 		fallthrough;
2475 	case NL80211_IFTYPE_AP:
2476 	case NL80211_IFTYPE_OCB:
2477 	case NL80211_IFTYPE_ADHOC:
2478 		if (is_multicast_ether_addr(skb->data)) {
2479 			*sta_out = ERR_PTR(-ENOENT);
2480 			return 0;
2481 		}
2482 		sta = sta_info_get_bss(sdata, skb->data);
2483 		break;
2484 #ifdef CONFIG_MAC80211_MESH
2485 	case NL80211_IFTYPE_MESH_POINT:
2486 		/* determined much later */
2487 		*sta_out = NULL;
2488 		return 0;
2489 #endif
2490 	case NL80211_IFTYPE_STATION:
2491 		if (sdata->wdev.wiphy->flags & WIPHY_FLAG_SUPPORTS_TDLS) {
2492 			sta = sta_info_get(sdata, skb->data);
2493 			if (sta && test_sta_flag(sta, WLAN_STA_TDLS_PEER)) {
2494 				if (test_sta_flag(sta,
2495 						  WLAN_STA_TDLS_PEER_AUTH)) {
2496 					*sta_out = sta;
2497 					return 0;
2498 				}
2499 
2500 				/*
2501 				 * TDLS link during setup - throw out frames to
2502 				 * peer. Allow TDLS-setup frames to unauthorized
2503 				 * peers for the special case of a link teardown
2504 				 * after a TDLS sta is removed due to being
2505 				 * unreachable.
2506 				 */
2507 				if (!ieee80211_is_tdls_setup(skb))
2508 					return -EINVAL;
2509 			}
2510 
2511 		}
2512 
2513 		sta = sta_info_get(sdata, sdata->vif.cfg.ap_addr);
2514 		if (!sta)
2515 			return -ENOLINK;
2516 		break;
2517 	default:
2518 		return -EINVAL;
2519 	}
2520 
2521 	*sta_out = sta ?: ERR_PTR(-ENOENT);
2522 	return 0;
2523 }
2524 
ieee80211_store_ack_skb(struct ieee80211_local * local,struct sk_buff * skb,u32 * info_flags,u64 * cookie)2525 static u16 ieee80211_store_ack_skb(struct ieee80211_local *local,
2526 				   struct sk_buff *skb,
2527 				   u32 *info_flags,
2528 				   u64 *cookie)
2529 {
2530 	struct sk_buff *ack_skb;
2531 	u16 info_id = 0;
2532 
2533 	if (skb->sk)
2534 		ack_skb = skb_clone_sk(skb);
2535 	else
2536 		ack_skb = skb_clone(skb, GFP_ATOMIC);
2537 
2538 	if (ack_skb) {
2539 		unsigned long flags;
2540 		int id;
2541 
2542 		spin_lock_irqsave(&local->ack_status_lock, flags);
2543 		id = idr_alloc(&local->ack_status_frames, ack_skb,
2544 			       1, 0x2000, GFP_ATOMIC);
2545 		spin_unlock_irqrestore(&local->ack_status_lock, flags);
2546 
2547 		if (id >= 0) {
2548 			info_id = id;
2549 			*info_flags |= IEEE80211_TX_CTL_REQ_TX_STATUS;
2550 			if (cookie) {
2551 				*cookie = ieee80211_mgmt_tx_cookie(local);
2552 				IEEE80211_SKB_CB(ack_skb)->ack.cookie = *cookie;
2553 			}
2554 		} else {
2555 			kfree_skb(ack_skb);
2556 		}
2557 	}
2558 
2559 	return info_id;
2560 }
2561 
2562 /**
2563  * ieee80211_build_hdr - build 802.11 header in the given frame
2564  * @sdata: virtual interface to build the header for
2565  * @skb: the skb to build the header in
2566  * @info_flags: skb flags to set
2567  * @sta: the station pointer
2568  * @ctrl_flags: info control flags to set
2569  * @cookie: cookie pointer to fill (if not %NULL)
2570  *
2571  * This function takes the skb with 802.3 header and reformats the header to
2572  * the appropriate IEEE 802.11 header based on which interface the packet is
2573  * being transmitted on.
2574  *
2575  * Note that this function also takes care of the TX status request and
2576  * potential unsharing of the SKB - this needs to be interleaved with the
2577  * header building.
2578  *
2579  * The function requires the read-side RCU lock held
2580  *
2581  * Returns: the (possibly reallocated) skb or an ERR_PTR() code
2582  */
ieee80211_build_hdr(struct ieee80211_sub_if_data * sdata,struct sk_buff * skb,u32 info_flags,struct sta_info * sta,u32 ctrl_flags,u64 * cookie)2583 static struct sk_buff *ieee80211_build_hdr(struct ieee80211_sub_if_data *sdata,
2584 					   struct sk_buff *skb, u32 info_flags,
2585 					   struct sta_info *sta, u32 ctrl_flags,
2586 					   u64 *cookie)
2587 {
2588 	struct ieee80211_local *local = sdata->local;
2589 	struct ieee80211_tx_info *info;
2590 	int head_need;
2591 	u16 ethertype, hdrlen,  meshhdrlen = 0;
2592 	__le16 fc;
2593 	struct ieee80211_hdr hdr;
2594 	struct ieee80211s_hdr mesh_hdr __maybe_unused;
2595 	struct mesh_path __maybe_unused *mppath = NULL, *mpath = NULL;
2596 	const u8 *encaps_data;
2597 	int encaps_len, skip_header_bytes;
2598 	bool wme_sta = false, authorized = false;
2599 	bool tdls_peer;
2600 	bool multicast;
2601 	u16 info_id = 0;
2602 	struct ieee80211_chanctx_conf *chanctx_conf = NULL;
2603 	enum nl80211_band band;
2604 	int ret;
2605 	u8 link_id = u32_get_bits(ctrl_flags, IEEE80211_TX_CTRL_MLO_LINK);
2606 
2607 	if (IS_ERR(sta))
2608 		sta = NULL;
2609 
2610 #ifdef CONFIG_MAC80211_DEBUGFS
2611 	if (local->force_tx_status)
2612 		info_flags |= IEEE80211_TX_CTL_REQ_TX_STATUS;
2613 #endif
2614 
2615 	/* convert Ethernet header to proper 802.11 header (based on
2616 	 * operation mode) */
2617 	ethertype = (skb->data[12] << 8) | skb->data[13];
2618 	fc = cpu_to_le16(IEEE80211_FTYPE_DATA | IEEE80211_STYPE_DATA);
2619 
2620 	if (!ieee80211_vif_is_mld(&sdata->vif))
2621 		chanctx_conf =
2622 			rcu_dereference(sdata->vif.bss_conf.chanctx_conf);
2623 
2624 	switch (sdata->vif.type) {
2625 	case NL80211_IFTYPE_AP_VLAN:
2626 		if (sdata->wdev.use_4addr) {
2627 			fc |= cpu_to_le16(IEEE80211_FCTL_FROMDS | IEEE80211_FCTL_TODS);
2628 			/* RA TA DA SA */
2629 			memcpy(hdr.addr1, sta->sta.addr, ETH_ALEN);
2630 			memcpy(hdr.addr2, sdata->vif.addr, ETH_ALEN);
2631 			memcpy(hdr.addr3, skb->data, ETH_ALEN);
2632 			memcpy(hdr.addr4, skb->data + ETH_ALEN, ETH_ALEN);
2633 			hdrlen = 30;
2634 			authorized = test_sta_flag(sta, WLAN_STA_AUTHORIZED);
2635 			wme_sta = sta->sta.wme;
2636 		}
2637 		if (!ieee80211_vif_is_mld(&sdata->vif)) {
2638 			struct ieee80211_sub_if_data *ap_sdata;
2639 
2640 			/* override chanctx_conf from AP (we don't have one) */
2641 			ap_sdata = container_of(sdata->bss,
2642 						struct ieee80211_sub_if_data,
2643 						u.ap);
2644 			chanctx_conf =
2645 				rcu_dereference(ap_sdata->vif.bss_conf.chanctx_conf);
2646 		}
2647 		if (sdata->wdev.use_4addr)
2648 			break;
2649 		fallthrough;
2650 	case NL80211_IFTYPE_AP:
2651 		fc |= cpu_to_le16(IEEE80211_FCTL_FROMDS);
2652 		/* DA BSSID SA */
2653 		memcpy(hdr.addr1, skb->data, ETH_ALEN);
2654 
2655 		if (ieee80211_vif_is_mld(&sdata->vif) && sta && !sta->sta.mlo) {
2656 			struct ieee80211_link_data *link;
2657 
2658 			link_id = sta->deflink.link_id;
2659 			link = rcu_dereference(sdata->link[link_id]);
2660 			if (WARN_ON(!link)) {
2661 				ret = -ENOLINK;
2662 				goto free;
2663 			}
2664 			memcpy(hdr.addr2, link->conf->addr, ETH_ALEN);
2665 		} else if (link_id == IEEE80211_LINK_UNSPECIFIED ||
2666 			   (sta && sta->sta.mlo)) {
2667 			memcpy(hdr.addr2, sdata->vif.addr, ETH_ALEN);
2668 		} else {
2669 			struct ieee80211_bss_conf *conf;
2670 
2671 			conf = rcu_dereference(sdata->vif.link_conf[link_id]);
2672 			if (unlikely(!conf)) {
2673 				ret = -ENOLINK;
2674 				goto free;
2675 			}
2676 
2677 			memcpy(hdr.addr2, conf->addr, ETH_ALEN);
2678 		}
2679 
2680 		memcpy(hdr.addr3, skb->data + ETH_ALEN, ETH_ALEN);
2681 		hdrlen = 24;
2682 		break;
2683 #ifdef CONFIG_MAC80211_MESH
2684 	case NL80211_IFTYPE_MESH_POINT:
2685 		if (!is_multicast_ether_addr(skb->data)) {
2686 			struct sta_info *next_hop;
2687 			bool mpp_lookup = true;
2688 
2689 			mpath = mesh_path_lookup(sdata, skb->data);
2690 			if (mpath) {
2691 				mpp_lookup = false;
2692 				next_hop = rcu_dereference(mpath->next_hop);
2693 				if (!next_hop ||
2694 				    !(mpath->flags & (MESH_PATH_ACTIVE |
2695 						      MESH_PATH_RESOLVING)))
2696 					mpp_lookup = true;
2697 			}
2698 
2699 			if (mpp_lookup) {
2700 				mppath = mpp_path_lookup(sdata, skb->data);
2701 				if (mppath)
2702 					mppath->exp_time = jiffies;
2703 			}
2704 
2705 			if (mppath && mpath)
2706 				mesh_path_del(sdata, mpath->dst);
2707 		}
2708 
2709 		/*
2710 		 * Use address extension if it is a packet from
2711 		 * another interface or if we know the destination
2712 		 * is being proxied by a portal (i.e. portal address
2713 		 * differs from proxied address)
2714 		 */
2715 		if (ether_addr_equal(sdata->vif.addr, skb->data + ETH_ALEN) &&
2716 		    !(mppath && !ether_addr_equal(mppath->mpp, skb->data))) {
2717 			hdrlen = ieee80211_fill_mesh_addresses(&hdr, &fc,
2718 					skb->data, skb->data + ETH_ALEN);
2719 			meshhdrlen = ieee80211_new_mesh_header(sdata, &mesh_hdr,
2720 							       NULL, NULL);
2721 		} else {
2722 			/* DS -> MBSS (802.11-2012 13.11.3.3).
2723 			 * For unicast with unknown forwarding information,
2724 			 * destination might be in the MBSS or if that fails
2725 			 * forwarded to another mesh gate. In either case
2726 			 * resolution will be handled in ieee80211_xmit(), so
2727 			 * leave the original DA. This also works for mcast */
2728 			const u8 *mesh_da = skb->data;
2729 
2730 			if (mppath)
2731 				mesh_da = mppath->mpp;
2732 			else if (mpath)
2733 				mesh_da = mpath->dst;
2734 
2735 			hdrlen = ieee80211_fill_mesh_addresses(&hdr, &fc,
2736 					mesh_da, sdata->vif.addr);
2737 			if (is_multicast_ether_addr(mesh_da))
2738 				/* DA TA mSA AE:SA */
2739 				meshhdrlen = ieee80211_new_mesh_header(
2740 						sdata, &mesh_hdr,
2741 						skb->data + ETH_ALEN, NULL);
2742 			else
2743 				/* RA TA mDA mSA AE:DA SA */
2744 				meshhdrlen = ieee80211_new_mesh_header(
2745 						sdata, &mesh_hdr, skb->data,
2746 						skb->data + ETH_ALEN);
2747 
2748 		}
2749 
2750 		/* For injected frames, fill RA right away as nexthop lookup
2751 		 * will be skipped.
2752 		 */
2753 		if ((ctrl_flags & IEEE80211_TX_CTRL_SKIP_MPATH_LOOKUP) &&
2754 		    is_zero_ether_addr(hdr.addr1))
2755 			memcpy(hdr.addr1, skb->data, ETH_ALEN);
2756 		break;
2757 #endif
2758 	case NL80211_IFTYPE_STATION:
2759 		/* we already did checks when looking up the RA STA */
2760 		tdls_peer = test_sta_flag(sta, WLAN_STA_TDLS_PEER);
2761 
2762 		if (tdls_peer) {
2763 			/* For TDLS only one link can be valid with peer STA */
2764 			int tdls_link_id = sta->sta.valid_links ?
2765 					   __ffs(sta->sta.valid_links) : 0;
2766 			struct ieee80211_link_data *link;
2767 
2768 			/* DA SA BSSID */
2769 			memcpy(hdr.addr1, skb->data, ETH_ALEN);
2770 			memcpy(hdr.addr2, skb->data + ETH_ALEN, ETH_ALEN);
2771 			link = rcu_dereference(sdata->link[tdls_link_id]);
2772 			if (WARN_ON_ONCE(!link)) {
2773 				ret = -EINVAL;
2774 				goto free;
2775 			}
2776 			memcpy(hdr.addr3, link->u.mgd.bssid, ETH_ALEN);
2777 			hdrlen = 24;
2778 		}  else if (sdata->u.mgd.use_4addr &&
2779 			    cpu_to_be16(ethertype) != sdata->control_port_protocol) {
2780 			fc |= cpu_to_le16(IEEE80211_FCTL_FROMDS |
2781 					  IEEE80211_FCTL_TODS);
2782 			/* RA TA DA SA */
2783 			memcpy(hdr.addr1, sdata->deflink.u.mgd.bssid, ETH_ALEN);
2784 			memcpy(hdr.addr2, sdata->vif.addr, ETH_ALEN);
2785 			memcpy(hdr.addr3, skb->data, ETH_ALEN);
2786 			memcpy(hdr.addr4, skb->data + ETH_ALEN, ETH_ALEN);
2787 			hdrlen = 30;
2788 		} else {
2789 			fc |= cpu_to_le16(IEEE80211_FCTL_TODS);
2790 			/* BSSID SA DA */
2791 			memcpy(hdr.addr1, sdata->vif.cfg.ap_addr, ETH_ALEN);
2792 			memcpy(hdr.addr2, skb->data + ETH_ALEN, ETH_ALEN);
2793 			memcpy(hdr.addr3, skb->data, ETH_ALEN);
2794 			hdrlen = 24;
2795 		}
2796 		break;
2797 	case NL80211_IFTYPE_OCB:
2798 		/* DA SA BSSID */
2799 		memcpy(hdr.addr1, skb->data, ETH_ALEN);
2800 		memcpy(hdr.addr2, skb->data + ETH_ALEN, ETH_ALEN);
2801 		eth_broadcast_addr(hdr.addr3);
2802 		hdrlen = 24;
2803 		break;
2804 	case NL80211_IFTYPE_ADHOC:
2805 		/* DA SA BSSID */
2806 		memcpy(hdr.addr1, skb->data, ETH_ALEN);
2807 		memcpy(hdr.addr2, skb->data + ETH_ALEN, ETH_ALEN);
2808 		memcpy(hdr.addr3, sdata->u.ibss.bssid, ETH_ALEN);
2809 		hdrlen = 24;
2810 		break;
2811 	default:
2812 		ret = -EINVAL;
2813 		goto free;
2814 	}
2815 
2816 	if (!chanctx_conf) {
2817 		if (!ieee80211_vif_is_mld(&sdata->vif)) {
2818 			ret = -ENOTCONN;
2819 			goto free;
2820 		}
2821 		/* MLD transmissions must not rely on the band */
2822 		band = 0;
2823 	} else {
2824 		band = chanctx_conf->def.chan->band;
2825 	}
2826 
2827 	multicast = is_multicast_ether_addr(hdr.addr1);
2828 
2829 	/* sta is always NULL for mesh */
2830 	if (sta) {
2831 		authorized = test_sta_flag(sta, WLAN_STA_AUTHORIZED);
2832 		wme_sta = sta->sta.wme;
2833 	} else if (ieee80211_vif_is_mesh(&sdata->vif)) {
2834 		/* For mesh, the use of the QoS header is mandatory */
2835 		wme_sta = true;
2836 	}
2837 
2838 	/* receiver does QoS (which also means we do) use it */
2839 	if (wme_sta) {
2840 		fc |= cpu_to_le16(IEEE80211_STYPE_QOS_DATA);
2841 		hdrlen += 2;
2842 	}
2843 
2844 	/*
2845 	 * Drop unicast frames to unauthorised stations unless they are
2846 	 * EAPOL frames from the local station.
2847 	 */
2848 	if (unlikely(!ieee80211_vif_is_mesh(&sdata->vif) &&
2849 		     (sdata->vif.type != NL80211_IFTYPE_OCB) &&
2850 		     !multicast && !authorized &&
2851 		     (cpu_to_be16(ethertype) != sdata->control_port_protocol ||
2852 		      !ieee80211_is_our_addr(sdata, skb->data + ETH_ALEN, NULL)))) {
2853 #ifdef CONFIG_MAC80211_VERBOSE_DEBUG
2854 		net_info_ratelimited("%s: dropped frame to %pM (unauthorized port)\n",
2855 				    sdata->name, hdr.addr1);
2856 #endif
2857 
2858 		I802_DEBUG_INC(local->tx_handlers_drop_unauth_port);
2859 
2860 		ret = -EPERM;
2861 		goto free;
2862 	}
2863 
2864 	if (unlikely(!multicast && ((skb->sk &&
2865 		     skb_shinfo(skb)->tx_flags & SKBTX_WIFI_STATUS) ||
2866 		     ctrl_flags & IEEE80211_TX_CTL_REQ_TX_STATUS)))
2867 		info_id = ieee80211_store_ack_skb(local, skb, &info_flags,
2868 						  cookie);
2869 
2870 	/*
2871 	 * If the skb is shared we need to obtain our own copy.
2872 	 */
2873 	skb = skb_share_check(skb, GFP_ATOMIC);
2874 	if (unlikely(!skb)) {
2875 		ret = -ENOMEM;
2876 		goto free;
2877 	}
2878 
2879 	hdr.frame_control = fc;
2880 	hdr.duration_id = 0;
2881 	hdr.seq_ctrl = 0;
2882 
2883 	skip_header_bytes = ETH_HLEN;
2884 	if (ethertype == ETH_P_AARP || ethertype == ETH_P_IPX) {
2885 		encaps_data = bridge_tunnel_header;
2886 		encaps_len = sizeof(bridge_tunnel_header);
2887 		skip_header_bytes -= 2;
2888 	} else if (ethertype >= ETH_P_802_3_MIN) {
2889 		encaps_data = rfc1042_header;
2890 		encaps_len = sizeof(rfc1042_header);
2891 		skip_header_bytes -= 2;
2892 	} else {
2893 		encaps_data = NULL;
2894 		encaps_len = 0;
2895 	}
2896 
2897 	skb_pull(skb, skip_header_bytes);
2898 	head_need = hdrlen + encaps_len + meshhdrlen - skb_headroom(skb);
2899 
2900 	/*
2901 	 * So we need to modify the skb header and hence need a copy of
2902 	 * that. The head_need variable above doesn't, so far, include
2903 	 * the needed header space that we don't need right away. If we
2904 	 * can, then we don't reallocate right now but only after the
2905 	 * frame arrives at the master device (if it does...)
2906 	 *
2907 	 * If we cannot, however, then we will reallocate to include all
2908 	 * the ever needed space. Also, if we need to reallocate it anyway,
2909 	 * make it big enough for everything we may ever need.
2910 	 */
2911 
2912 	if (head_need > 0 || skb_cloned(skb)) {
2913 		head_need += IEEE80211_ENCRYPT_HEADROOM;
2914 		head_need += local->tx_headroom;
2915 		head_need = max_t(int, 0, head_need);
2916 		if (ieee80211_skb_resize(sdata, skb, head_need, ENCRYPT_DATA)) {
2917 			ieee80211_free_txskb(&local->hw, skb);
2918 			skb = NULL;
2919 			return ERR_PTR(-ENOMEM);
2920 		}
2921 	}
2922 
2923 	if (encaps_data)
2924 		memcpy(skb_push(skb, encaps_len), encaps_data, encaps_len);
2925 
2926 #ifdef CONFIG_MAC80211_MESH
2927 	if (meshhdrlen > 0)
2928 		memcpy(skb_push(skb, meshhdrlen), &mesh_hdr, meshhdrlen);
2929 #endif
2930 
2931 	if (ieee80211_is_data_qos(fc)) {
2932 		__le16 *qos_control;
2933 
2934 		qos_control = skb_push(skb, 2);
2935 		memcpy(skb_push(skb, hdrlen - 2), &hdr, hdrlen - 2);
2936 		/*
2937 		 * Maybe we could actually set some fields here, for now just
2938 		 * initialise to zero to indicate no special operation.
2939 		 */
2940 		*qos_control = 0;
2941 	} else
2942 		memcpy(skb_push(skb, hdrlen), &hdr, hdrlen);
2943 
2944 	skb_reset_mac_header(skb);
2945 
2946 	info = IEEE80211_SKB_CB(skb);
2947 	memset(info, 0, sizeof(*info));
2948 
2949 	info->flags = info_flags;
2950 	info->ack_frame_id = info_id;
2951 	info->band = band;
2952 
2953 	if (likely(!cookie)) {
2954 		ctrl_flags |= u32_encode_bits(link_id,
2955 					      IEEE80211_TX_CTRL_MLO_LINK);
2956 	} else {
2957 		unsigned int pre_conf_link_id;
2958 
2959 		/*
2960 		 * ctrl_flags already have been set by
2961 		 * ieee80211_tx_control_port(), here
2962 		 * we just sanity check that
2963 		 */
2964 
2965 		pre_conf_link_id = u32_get_bits(ctrl_flags,
2966 						IEEE80211_TX_CTRL_MLO_LINK);
2967 
2968 		if (pre_conf_link_id != link_id &&
2969 		    link_id != IEEE80211_LINK_UNSPECIFIED) {
2970 #ifdef CONFIG_MAC80211_VERBOSE_DEBUG
2971 			net_info_ratelimited("%s: dropped frame to %pM with bad link ID request (%d vs. %d)\n",
2972 					     sdata->name, hdr.addr1,
2973 					     pre_conf_link_id, link_id);
2974 #endif
2975 			ret = -EINVAL;
2976 			goto free;
2977 		}
2978 	}
2979 
2980 	info->control.flags = ctrl_flags;
2981 
2982 	return skb;
2983  free:
2984 	kfree_skb(skb);
2985 	return ERR_PTR(ret);
2986 }
2987 
2988 /*
2989  * fast-xmit overview
2990  *
2991  * The core idea of this fast-xmit is to remove per-packet checks by checking
2992  * them out of band. ieee80211_check_fast_xmit() implements the out-of-band
2993  * checks that are needed to get the sta->fast_tx pointer assigned, after which
2994  * much less work can be done per packet. For example, fragmentation must be
2995  * disabled or the fast_tx pointer will not be set. All the conditions are seen
2996  * in the code here.
2997  *
2998  * Once assigned, the fast_tx data structure also caches the per-packet 802.11
2999  * header and other data to aid packet processing in ieee80211_xmit_fast().
3000  *
3001  * The most difficult part of this is that when any of these assumptions
3002  * change, an external trigger (i.e. a call to ieee80211_clear_fast_xmit(),
3003  * ieee80211_check_fast_xmit() or friends) is required to reset the data,
3004  * since the per-packet code no longer checks the conditions. This is reflected
3005  * by the calls to these functions throughout the rest of the code, and must be
3006  * maintained if any of the TX path checks change.
3007  */
3008 
ieee80211_check_fast_xmit(struct sta_info * sta)3009 void ieee80211_check_fast_xmit(struct sta_info *sta)
3010 {
3011 	struct ieee80211_fast_tx build = {}, *fast_tx = NULL, *old;
3012 	struct ieee80211_local *local = sta->local;
3013 	struct ieee80211_sub_if_data *sdata = sta->sdata;
3014 	struct ieee80211_hdr *hdr = (void *)build.hdr;
3015 	struct ieee80211_chanctx_conf *chanctx_conf;
3016 	__le16 fc;
3017 
3018 	if (!ieee80211_hw_check(&local->hw, SUPPORT_FAST_XMIT))
3019 		return;
3020 
3021 	if (ieee80211_vif_is_mesh(&sdata->vif))
3022 		mesh_fast_tx_flush_sta(sdata, sta);
3023 
3024 	/* Locking here protects both the pointer itself, and against concurrent
3025 	 * invocations winning data access races to, e.g., the key pointer that
3026 	 * is used.
3027 	 * Without it, the invocation of this function right after the key
3028 	 * pointer changes wouldn't be sufficient, as another CPU could access
3029 	 * the pointer, then stall, and then do the cache update after the CPU
3030 	 * that invalidated the key.
3031 	 * With the locking, such scenarios cannot happen as the check for the
3032 	 * key and the fast-tx assignment are done atomically, so the CPU that
3033 	 * modifies the key will either wait or other one will see the key
3034 	 * cleared/changed already.
3035 	 */
3036 	spin_lock_bh(&sta->lock);
3037 	if (ieee80211_hw_check(&local->hw, SUPPORTS_PS) &&
3038 	    !ieee80211_hw_check(&local->hw, SUPPORTS_DYNAMIC_PS) &&
3039 	    sdata->vif.type == NL80211_IFTYPE_STATION)
3040 		goto out;
3041 
3042 	if (!test_sta_flag(sta, WLAN_STA_AUTHORIZED) || !sta->uploaded)
3043 		goto out;
3044 
3045 	if (test_sta_flag(sta, WLAN_STA_PS_STA) ||
3046 	    test_sta_flag(sta, WLAN_STA_PS_DRIVER) ||
3047 	    test_sta_flag(sta, WLAN_STA_PS_DELIVER) ||
3048 	    test_sta_flag(sta, WLAN_STA_CLEAR_PS_FILT))
3049 		goto out;
3050 
3051 	if (sdata->noack_map)
3052 		goto out;
3053 
3054 	/* fast-xmit doesn't handle fragmentation at all */
3055 	if (local->hw.wiphy->frag_threshold != (u32)-1 &&
3056 	    !ieee80211_hw_check(&local->hw, SUPPORTS_TX_FRAG))
3057 		goto out;
3058 
3059 	if (!ieee80211_vif_is_mld(&sdata->vif)) {
3060 		rcu_read_lock();
3061 		chanctx_conf =
3062 			rcu_dereference(sdata->vif.bss_conf.chanctx_conf);
3063 		if (!chanctx_conf) {
3064 			rcu_read_unlock();
3065 			goto out;
3066 		}
3067 		build.band = chanctx_conf->def.chan->band;
3068 		rcu_read_unlock();
3069 	} else {
3070 		/* MLD transmissions must not rely on the band */
3071 		build.band = 0;
3072 	}
3073 
3074 	fc = cpu_to_le16(IEEE80211_FTYPE_DATA | IEEE80211_STYPE_DATA);
3075 
3076 	switch (sdata->vif.type) {
3077 	case NL80211_IFTYPE_ADHOC:
3078 		/* DA SA BSSID */
3079 		build.da_offs = offsetof(struct ieee80211_hdr, addr1);
3080 		build.sa_offs = offsetof(struct ieee80211_hdr, addr2);
3081 		memcpy(hdr->addr3, sdata->u.ibss.bssid, ETH_ALEN);
3082 		build.hdr_len = 24;
3083 		break;
3084 	case NL80211_IFTYPE_STATION:
3085 		if (test_sta_flag(sta, WLAN_STA_TDLS_PEER)) {
3086 			/* For TDLS only one link can be valid with peer STA */
3087 			int tdls_link_id = sta->sta.valid_links ?
3088 					   __ffs(sta->sta.valid_links) : 0;
3089 			struct ieee80211_link_data *link;
3090 
3091 			/* DA SA BSSID */
3092 			build.da_offs = offsetof(struct ieee80211_hdr, addr1);
3093 			build.sa_offs = offsetof(struct ieee80211_hdr, addr2);
3094 			rcu_read_lock();
3095 			link = rcu_dereference(sdata->link[tdls_link_id]);
3096 			if (!WARN_ON_ONCE(!link))
3097 				memcpy(hdr->addr3, link->u.mgd.bssid, ETH_ALEN);
3098 			rcu_read_unlock();
3099 			build.hdr_len = 24;
3100 			break;
3101 		}
3102 
3103 		if (sdata->u.mgd.use_4addr) {
3104 			/* non-regular ethertype cannot use the fastpath */
3105 			fc |= cpu_to_le16(IEEE80211_FCTL_FROMDS |
3106 					  IEEE80211_FCTL_TODS);
3107 			/* RA TA DA SA */
3108 			memcpy(hdr->addr1, sdata->deflink.u.mgd.bssid, ETH_ALEN);
3109 			memcpy(hdr->addr2, sdata->vif.addr, ETH_ALEN);
3110 			build.da_offs = offsetof(struct ieee80211_hdr, addr3);
3111 			build.sa_offs = offsetof(struct ieee80211_hdr, addr4);
3112 			build.hdr_len = 30;
3113 			break;
3114 		}
3115 		fc |= cpu_to_le16(IEEE80211_FCTL_TODS);
3116 		/* BSSID SA DA */
3117 		memcpy(hdr->addr1, sdata->vif.cfg.ap_addr, ETH_ALEN);
3118 		build.da_offs = offsetof(struct ieee80211_hdr, addr3);
3119 		build.sa_offs = offsetof(struct ieee80211_hdr, addr2);
3120 		build.hdr_len = 24;
3121 		break;
3122 	case NL80211_IFTYPE_AP_VLAN:
3123 		if (sdata->wdev.use_4addr) {
3124 			fc |= cpu_to_le16(IEEE80211_FCTL_FROMDS |
3125 					  IEEE80211_FCTL_TODS);
3126 			/* RA TA DA SA */
3127 			memcpy(hdr->addr1, sta->sta.addr, ETH_ALEN);
3128 			memcpy(hdr->addr2, sdata->vif.addr, ETH_ALEN);
3129 			build.da_offs = offsetof(struct ieee80211_hdr, addr3);
3130 			build.sa_offs = offsetof(struct ieee80211_hdr, addr4);
3131 			build.hdr_len = 30;
3132 			break;
3133 		}
3134 		fallthrough;
3135 	case NL80211_IFTYPE_AP:
3136 		fc |= cpu_to_le16(IEEE80211_FCTL_FROMDS);
3137 		/* DA BSSID SA */
3138 		build.da_offs = offsetof(struct ieee80211_hdr, addr1);
3139 		if (sta->sta.mlo || !ieee80211_vif_is_mld(&sdata->vif)) {
3140 			memcpy(hdr->addr2, sdata->vif.addr, ETH_ALEN);
3141 		} else {
3142 			unsigned int link_id = sta->deflink.link_id;
3143 			struct ieee80211_link_data *link;
3144 
3145 			rcu_read_lock();
3146 			link = rcu_dereference(sdata->link[link_id]);
3147 			if (WARN_ON(!link)) {
3148 				rcu_read_unlock();
3149 				goto out;
3150 			}
3151 			memcpy(hdr->addr2, link->conf->addr, ETH_ALEN);
3152 			rcu_read_unlock();
3153 		}
3154 		build.sa_offs = offsetof(struct ieee80211_hdr, addr3);
3155 		build.hdr_len = 24;
3156 		break;
3157 	default:
3158 		/* not handled on fast-xmit */
3159 		goto out;
3160 	}
3161 
3162 	if (sta->sta.wme) {
3163 		build.hdr_len += 2;
3164 		fc |= cpu_to_le16(IEEE80211_STYPE_QOS_DATA);
3165 	}
3166 
3167 	/* We store the key here so there's no point in using rcu_dereference()
3168 	 * but that's fine because the code that changes the pointers will call
3169 	 * this function after doing so. For a single CPU that would be enough,
3170 	 * for multiple see the comment above.
3171 	 */
3172 	build.key = rcu_access_pointer(sta->ptk[sta->ptk_idx]);
3173 	if (!build.key)
3174 		build.key = rcu_access_pointer(sdata->default_unicast_key);
3175 	if (build.key) {
3176 		bool gen_iv, iv_spc, mmic;
3177 
3178 		gen_iv = build.key->conf.flags & IEEE80211_KEY_FLAG_GENERATE_IV;
3179 		iv_spc = build.key->conf.flags & IEEE80211_KEY_FLAG_PUT_IV_SPACE;
3180 		mmic = build.key->conf.flags &
3181 			(IEEE80211_KEY_FLAG_GENERATE_MMIC |
3182 			 IEEE80211_KEY_FLAG_PUT_MIC_SPACE);
3183 
3184 		/* don't handle software crypto */
3185 		if (!(build.key->flags & KEY_FLAG_UPLOADED_TO_HARDWARE))
3186 			goto out;
3187 
3188 		/* Key is being removed */
3189 		if (build.key->flags & KEY_FLAG_TAINTED)
3190 			goto out;
3191 
3192 		switch (build.key->conf.cipher) {
3193 		case WLAN_CIPHER_SUITE_CCMP:
3194 		case WLAN_CIPHER_SUITE_CCMP_256:
3195 			if (gen_iv)
3196 				build.pn_offs = build.hdr_len;
3197 			if (gen_iv || iv_spc)
3198 				build.hdr_len += IEEE80211_CCMP_HDR_LEN;
3199 			break;
3200 		case WLAN_CIPHER_SUITE_GCMP:
3201 		case WLAN_CIPHER_SUITE_GCMP_256:
3202 			if (gen_iv)
3203 				build.pn_offs = build.hdr_len;
3204 			if (gen_iv || iv_spc)
3205 				build.hdr_len += IEEE80211_GCMP_HDR_LEN;
3206 			break;
3207 		case WLAN_CIPHER_SUITE_TKIP:
3208 			/* cannot handle MMIC or IV generation in xmit-fast */
3209 			if (mmic || gen_iv)
3210 				goto out;
3211 			if (iv_spc)
3212 				build.hdr_len += IEEE80211_TKIP_IV_LEN;
3213 			break;
3214 		case WLAN_CIPHER_SUITE_WEP40:
3215 		case WLAN_CIPHER_SUITE_WEP104:
3216 			/* cannot handle IV generation in fast-xmit */
3217 			if (gen_iv)
3218 				goto out;
3219 			if (iv_spc)
3220 				build.hdr_len += IEEE80211_WEP_IV_LEN;
3221 			break;
3222 		case WLAN_CIPHER_SUITE_AES_CMAC:
3223 		case WLAN_CIPHER_SUITE_BIP_CMAC_256:
3224 		case WLAN_CIPHER_SUITE_BIP_GMAC_128:
3225 		case WLAN_CIPHER_SUITE_BIP_GMAC_256:
3226 			WARN(1,
3227 			     "management cipher suite 0x%x enabled for data\n",
3228 			     build.key->conf.cipher);
3229 			goto out;
3230 		default:
3231 			/* we don't know how to generate IVs for this at all */
3232 			if (WARN_ON(gen_iv))
3233 				goto out;
3234 		}
3235 
3236 		fc |= cpu_to_le16(IEEE80211_FCTL_PROTECTED);
3237 	}
3238 
3239 	hdr->frame_control = fc;
3240 
3241 	memcpy(build.hdr + build.hdr_len,
3242 	       rfc1042_header,  sizeof(rfc1042_header));
3243 	build.hdr_len += sizeof(rfc1042_header);
3244 
3245 	fast_tx = kmemdup(&build, sizeof(build), GFP_ATOMIC);
3246 	/* if the kmemdup fails, continue w/o fast_tx */
3247 
3248  out:
3249 	/* we might have raced against another call to this function */
3250 	old = rcu_dereference_protected(sta->fast_tx,
3251 					lockdep_is_held(&sta->lock));
3252 	rcu_assign_pointer(sta->fast_tx, fast_tx);
3253 	if (old)
3254 		kfree_rcu(old, rcu_head);
3255 	spin_unlock_bh(&sta->lock);
3256 }
3257 
ieee80211_check_fast_xmit_all(struct ieee80211_local * local)3258 void ieee80211_check_fast_xmit_all(struct ieee80211_local *local)
3259 {
3260 	struct sta_info *sta;
3261 
3262 	rcu_read_lock();
3263 	list_for_each_entry_rcu(sta, &local->sta_list, list)
3264 		ieee80211_check_fast_xmit(sta);
3265 	rcu_read_unlock();
3266 }
3267 
ieee80211_check_fast_xmit_iface(struct ieee80211_sub_if_data * sdata)3268 void ieee80211_check_fast_xmit_iface(struct ieee80211_sub_if_data *sdata)
3269 {
3270 	struct ieee80211_local *local = sdata->local;
3271 	struct sta_info *sta;
3272 
3273 	rcu_read_lock();
3274 
3275 	list_for_each_entry_rcu(sta, &local->sta_list, list) {
3276 		if (sdata != sta->sdata &&
3277 		    (!sta->sdata->bss || sta->sdata->bss != sdata->bss))
3278 			continue;
3279 		ieee80211_check_fast_xmit(sta);
3280 	}
3281 
3282 	rcu_read_unlock();
3283 }
3284 
ieee80211_clear_fast_xmit(struct sta_info * sta)3285 void ieee80211_clear_fast_xmit(struct sta_info *sta)
3286 {
3287 	struct ieee80211_fast_tx *fast_tx;
3288 
3289 	spin_lock_bh(&sta->lock);
3290 	fast_tx = rcu_dereference_protected(sta->fast_tx,
3291 					    lockdep_is_held(&sta->lock));
3292 	RCU_INIT_POINTER(sta->fast_tx, NULL);
3293 	spin_unlock_bh(&sta->lock);
3294 
3295 	if (fast_tx)
3296 		kfree_rcu(fast_tx, rcu_head);
3297 }
3298 
ieee80211_amsdu_realloc_pad(struct ieee80211_local * local,struct sk_buff * skb,int headroom)3299 static bool ieee80211_amsdu_realloc_pad(struct ieee80211_local *local,
3300 					struct sk_buff *skb, int headroom)
3301 {
3302 	if (skb_headroom(skb) < headroom) {
3303 		I802_DEBUG_INC(local->tx_expand_skb_head);
3304 
3305 		if (pskb_expand_head(skb, headroom, 0, GFP_ATOMIC)) {
3306 			wiphy_debug(local->hw.wiphy,
3307 				    "failed to reallocate TX buffer\n");
3308 			return false;
3309 		}
3310 	}
3311 
3312 	return true;
3313 }
3314 
ieee80211_amsdu_prepare_head(struct ieee80211_sub_if_data * sdata,struct ieee80211_fast_tx * fast_tx,struct sk_buff * skb)3315 static bool ieee80211_amsdu_prepare_head(struct ieee80211_sub_if_data *sdata,
3316 					 struct ieee80211_fast_tx *fast_tx,
3317 					 struct sk_buff *skb)
3318 {
3319 	struct ieee80211_local *local = sdata->local;
3320 	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
3321 	struct ieee80211_hdr *hdr;
3322 	struct ethhdr *amsdu_hdr;
3323 	int hdr_len = fast_tx->hdr_len - sizeof(rfc1042_header);
3324 	int subframe_len = skb->len - hdr_len;
3325 	void *data;
3326 	u8 *qc, *h_80211_src, *h_80211_dst;
3327 	const u8 *bssid;
3328 
3329 	if (info->flags & IEEE80211_TX_CTL_RATE_CTRL_PROBE)
3330 		return false;
3331 
3332 	if (info->control.flags & IEEE80211_TX_CTRL_AMSDU)
3333 		return true;
3334 
3335 	if (!ieee80211_amsdu_realloc_pad(local, skb,
3336 					 sizeof(*amsdu_hdr) +
3337 					 local->hw.extra_tx_headroom))
3338 		return false;
3339 
3340 	data = skb_push(skb, sizeof(*amsdu_hdr));
3341 	memmove(data, data + sizeof(*amsdu_hdr), hdr_len);
3342 	hdr = data;
3343 	amsdu_hdr = data + hdr_len;
3344 	/* h_80211_src/dst is addr* field within hdr */
3345 	h_80211_src = data + fast_tx->sa_offs;
3346 	h_80211_dst = data + fast_tx->da_offs;
3347 
3348 	amsdu_hdr->h_proto = cpu_to_be16(subframe_len);
3349 	ether_addr_copy(amsdu_hdr->h_source, h_80211_src);
3350 	ether_addr_copy(amsdu_hdr->h_dest, h_80211_dst);
3351 
3352 	/* according to IEEE 802.11-2012 8.3.2 table 8-19, the outer SA/DA
3353 	 * fields needs to be changed to BSSID for A-MSDU frames depending
3354 	 * on FromDS/ToDS values.
3355 	 */
3356 	switch (sdata->vif.type) {
3357 	case NL80211_IFTYPE_STATION:
3358 		bssid = sdata->vif.cfg.ap_addr;
3359 		break;
3360 	case NL80211_IFTYPE_AP:
3361 	case NL80211_IFTYPE_AP_VLAN:
3362 		bssid = sdata->vif.addr;
3363 		break;
3364 	default:
3365 		bssid = NULL;
3366 	}
3367 
3368 	if (bssid && ieee80211_has_fromds(hdr->frame_control))
3369 		ether_addr_copy(h_80211_src, bssid);
3370 
3371 	if (bssid && ieee80211_has_tods(hdr->frame_control))
3372 		ether_addr_copy(h_80211_dst, bssid);
3373 
3374 	qc = ieee80211_get_qos_ctl(hdr);
3375 	*qc |= IEEE80211_QOS_CTL_A_MSDU_PRESENT;
3376 
3377 	info->control.flags |= IEEE80211_TX_CTRL_AMSDU;
3378 
3379 	return true;
3380 }
3381 
ieee80211_amsdu_aggregate(struct ieee80211_sub_if_data * sdata,struct sta_info * sta,struct ieee80211_fast_tx * fast_tx,struct sk_buff * skb,const u8 * da,const u8 * sa)3382 static bool ieee80211_amsdu_aggregate(struct ieee80211_sub_if_data *sdata,
3383 				      struct sta_info *sta,
3384 				      struct ieee80211_fast_tx *fast_tx,
3385 				      struct sk_buff *skb,
3386 				      const u8 *da, const u8 *sa)
3387 {
3388 	struct ieee80211_local *local = sdata->local;
3389 	struct fq *fq = &local->fq;
3390 	struct fq_tin *tin;
3391 	struct fq_flow *flow;
3392 	u8 tid = skb->priority & IEEE80211_QOS_CTL_TAG1D_MASK;
3393 	struct ieee80211_txq *txq = sta->sta.txq[tid];
3394 	struct txq_info *txqi;
3395 	struct sk_buff **frag_tail, *head;
3396 	int subframe_len = skb->len - ETH_ALEN;
3397 	u8 max_subframes = sta->sta.max_amsdu_subframes;
3398 	int max_frags = local->hw.max_tx_fragments;
3399 	int max_amsdu_len = sta->sta.cur->max_amsdu_len;
3400 	int orig_truesize;
3401 	u32 flow_idx;
3402 	__be16 len;
3403 	void *data;
3404 	bool ret = false;
3405 	unsigned int orig_len;
3406 	int n = 2, nfrags, pad = 0;
3407 	u16 hdrlen;
3408 
3409 	if (!ieee80211_hw_check(&local->hw, TX_AMSDU))
3410 		return false;
3411 
3412 	if (sdata->vif.offload_flags & IEEE80211_OFFLOAD_ENCAP_ENABLED)
3413 		return false;
3414 
3415 	if (ieee80211_vif_is_mesh(&sdata->vif))
3416 		return false;
3417 
3418 	if (skb_is_gso(skb))
3419 		return false;
3420 
3421 	if (!txq)
3422 		return false;
3423 
3424 	txqi = to_txq_info(txq);
3425 	if (test_bit(IEEE80211_TXQ_NO_AMSDU, &txqi->flags))
3426 		return false;
3427 
3428 	if (sta->sta.cur->max_rc_amsdu_len)
3429 		max_amsdu_len = min_t(int, max_amsdu_len,
3430 				      sta->sta.cur->max_rc_amsdu_len);
3431 
3432 	if (sta->sta.cur->max_tid_amsdu_len[tid])
3433 		max_amsdu_len = min_t(int, max_amsdu_len,
3434 				      sta->sta.cur->max_tid_amsdu_len[tid]);
3435 
3436 	flow_idx = fq_flow_idx(fq, skb);
3437 
3438 	spin_lock_bh(&fq->lock);
3439 
3440 	/* TODO: Ideally aggregation should be done on dequeue to remain
3441 	 * responsive to environment changes.
3442 	 */
3443 
3444 	tin = &txqi->tin;
3445 	flow = fq_flow_classify(fq, tin, flow_idx, skb);
3446 	head = skb_peek_tail(&flow->queue);
3447 	if (!head || skb_is_gso(head))
3448 		goto out;
3449 
3450 	orig_truesize = head->truesize;
3451 	orig_len = head->len;
3452 
3453 	if (skb->len + head->len > max_amsdu_len)
3454 		goto out;
3455 
3456 	nfrags = 1 + skb_shinfo(skb)->nr_frags;
3457 	nfrags += 1 + skb_shinfo(head)->nr_frags;
3458 	frag_tail = &skb_shinfo(head)->frag_list;
3459 	while (*frag_tail) {
3460 		nfrags += 1 + skb_shinfo(*frag_tail)->nr_frags;
3461 		frag_tail = &(*frag_tail)->next;
3462 		n++;
3463 	}
3464 
3465 	if (max_subframes && n > max_subframes)
3466 		goto out;
3467 
3468 	if (max_frags && nfrags > max_frags)
3469 		goto out;
3470 
3471 	if (!drv_can_aggregate_in_amsdu(local, head, skb))
3472 		goto out;
3473 
3474 	if (!ieee80211_amsdu_prepare_head(sdata, fast_tx, head))
3475 		goto out;
3476 
3477 	/* If n == 2, the "while (*frag_tail)" loop above didn't execute
3478 	 * and  frag_tail should be &skb_shinfo(head)->frag_list.
3479 	 * However, ieee80211_amsdu_prepare_head() can reallocate it.
3480 	 * Reload frag_tail to have it pointing to the correct place.
3481 	 */
3482 	if (n == 2)
3483 		frag_tail = &skb_shinfo(head)->frag_list;
3484 
3485 	/*
3486 	 * Pad out the previous subframe to a multiple of 4 by adding the
3487 	 * padding to the next one, that's being added. Note that head->len
3488 	 * is the length of the full A-MSDU, but that works since each time
3489 	 * we add a new subframe we pad out the previous one to a multiple
3490 	 * of 4 and thus it no longer matters in the next round.
3491 	 */
3492 	hdrlen = fast_tx->hdr_len - sizeof(rfc1042_header);
3493 	if ((head->len - hdrlen) & 3)
3494 		pad = 4 - ((head->len - hdrlen) & 3);
3495 
3496 	if (!ieee80211_amsdu_realloc_pad(local, skb, sizeof(rfc1042_header) +
3497 						     2 + pad))
3498 		goto out_recalc;
3499 
3500 	ret = true;
3501 	data = skb_push(skb, ETH_ALEN + 2);
3502 	ether_addr_copy(data, da);
3503 	ether_addr_copy(data + ETH_ALEN, sa);
3504 
3505 	data += 2 * ETH_ALEN;
3506 	len = cpu_to_be16(subframe_len);
3507 	memcpy(data, &len, 2);
3508 	memcpy(data + 2, rfc1042_header, sizeof(rfc1042_header));
3509 
3510 	memset(skb_push(skb, pad), 0, pad);
3511 
3512 	head->len += skb->len;
3513 	head->data_len += skb->len;
3514 	*frag_tail = skb;
3515 
3516 out_recalc:
3517 	fq->memory_usage += head->truesize - orig_truesize;
3518 	if (head->len != orig_len) {
3519 		flow->backlog += head->len - orig_len;
3520 		tin->backlog_bytes += head->len - orig_len;
3521 	}
3522 out:
3523 	spin_unlock_bh(&fq->lock);
3524 
3525 	return ret;
3526 }
3527 
3528 /*
3529  * Can be called while the sta lock is held. Anything that can cause packets to
3530  * be generated will cause deadlock!
3531  */
3532 static ieee80211_tx_result
ieee80211_xmit_fast_finish(struct ieee80211_sub_if_data * sdata,struct sta_info * sta,u8 pn_offs,struct ieee80211_key * key,struct ieee80211_tx_data * tx)3533 ieee80211_xmit_fast_finish(struct ieee80211_sub_if_data *sdata,
3534 			   struct sta_info *sta, u8 pn_offs,
3535 			   struct ieee80211_key *key,
3536 			   struct ieee80211_tx_data *tx)
3537 {
3538 	struct sk_buff *skb = tx->skb;
3539 	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
3540 	struct ieee80211_hdr *hdr = (void *)skb->data;
3541 	u8 tid = IEEE80211_NUM_TIDS;
3542 
3543 	if (!ieee80211_hw_check(&tx->local->hw, HAS_RATE_CONTROL) &&
3544 	    ieee80211_tx_h_rate_ctrl(tx) != TX_CONTINUE)
3545 		return TX_DROP;
3546 
3547 	if (key)
3548 		info->control.hw_key = &key->conf;
3549 
3550 	dev_sw_netstats_tx_add(skb->dev, 1, skb->len);
3551 
3552 	if (hdr->frame_control & cpu_to_le16(IEEE80211_STYPE_QOS_DATA)) {
3553 		tid = skb->priority & IEEE80211_QOS_CTL_TAG1D_MASK;
3554 		hdr->seq_ctrl = ieee80211_tx_next_seq(sta, tid);
3555 	} else {
3556 		info->flags |= IEEE80211_TX_CTL_ASSIGN_SEQ;
3557 		hdr->seq_ctrl = cpu_to_le16(sdata->sequence_number);
3558 		sdata->sequence_number += 0x10;
3559 	}
3560 
3561 	if (skb_shinfo(skb)->gso_size)
3562 		sta->deflink.tx_stats.msdu[tid] +=
3563 			DIV_ROUND_UP(skb->len, skb_shinfo(skb)->gso_size);
3564 	else
3565 		sta->deflink.tx_stats.msdu[tid]++;
3566 
3567 	info->hw_queue = sdata->vif.hw_queue[skb_get_queue_mapping(skb)];
3568 
3569 	/* statistics normally done by ieee80211_tx_h_stats (but that
3570 	 * has to consider fragmentation, so is more complex)
3571 	 */
3572 	sta->deflink.tx_stats.bytes[skb_get_queue_mapping(skb)] += skb->len;
3573 	sta->deflink.tx_stats.packets[skb_get_queue_mapping(skb)]++;
3574 
3575 	if (pn_offs) {
3576 		u64 pn;
3577 		u8 *crypto_hdr = skb->data + pn_offs;
3578 
3579 		switch (key->conf.cipher) {
3580 		case WLAN_CIPHER_SUITE_CCMP:
3581 		case WLAN_CIPHER_SUITE_CCMP_256:
3582 		case WLAN_CIPHER_SUITE_GCMP:
3583 		case WLAN_CIPHER_SUITE_GCMP_256:
3584 			pn = atomic64_inc_return(&key->conf.tx_pn);
3585 			crypto_hdr[0] = pn;
3586 			crypto_hdr[1] = pn >> 8;
3587 			crypto_hdr[3] = 0x20 | (key->conf.keyidx << 6);
3588 			crypto_hdr[4] = pn >> 16;
3589 			crypto_hdr[5] = pn >> 24;
3590 			crypto_hdr[6] = pn >> 32;
3591 			crypto_hdr[7] = pn >> 40;
3592 			break;
3593 		}
3594 	}
3595 
3596 	return TX_CONTINUE;
3597 }
3598 
3599 static netdev_features_t
ieee80211_sdata_netdev_features(struct ieee80211_sub_if_data * sdata)3600 ieee80211_sdata_netdev_features(struct ieee80211_sub_if_data *sdata)
3601 {
3602 	if (sdata->vif.type != NL80211_IFTYPE_AP_VLAN)
3603 		return sdata->vif.netdev_features;
3604 
3605 	if (!sdata->bss)
3606 		return 0;
3607 
3608 	sdata = container_of(sdata->bss, struct ieee80211_sub_if_data, u.ap);
3609 	return sdata->vif.netdev_features;
3610 }
3611 
3612 static struct sk_buff *
ieee80211_tx_skb_fixup(struct sk_buff * skb,netdev_features_t features)3613 ieee80211_tx_skb_fixup(struct sk_buff *skb, netdev_features_t features)
3614 {
3615 	if (skb_is_gso(skb)) {
3616 		struct sk_buff *segs;
3617 
3618 		segs = skb_gso_segment(skb, features);
3619 		if (!segs)
3620 			return skb;
3621 		if (IS_ERR(segs))
3622 			goto free;
3623 
3624 		consume_skb(skb);
3625 		return segs;
3626 	}
3627 
3628 	if (skb_needs_linearize(skb, features) && __skb_linearize(skb))
3629 		goto free;
3630 
3631 	if (skb->ip_summed == CHECKSUM_PARTIAL) {
3632 		int ofs = skb_checksum_start_offset(skb);
3633 
3634 		if (skb->encapsulation)
3635 			skb_set_inner_transport_header(skb, ofs);
3636 		else
3637 			skb_set_transport_header(skb, ofs);
3638 
3639 		if (skb_csum_hwoffload_help(skb, features))
3640 			goto free;
3641 	}
3642 
3643 	skb_mark_not_on_list(skb);
3644 	return skb;
3645 
3646 free:
3647 	kfree_skb(skb);
3648 	return NULL;
3649 }
3650 
__ieee80211_xmit_fast(struct ieee80211_sub_if_data * sdata,struct sta_info * sta,struct ieee80211_fast_tx * fast_tx,struct sk_buff * skb,bool ampdu,const u8 * da,const u8 * sa)3651 void __ieee80211_xmit_fast(struct ieee80211_sub_if_data *sdata,
3652 			   struct sta_info *sta,
3653 			   struct ieee80211_fast_tx *fast_tx,
3654 			   struct sk_buff *skb, bool ampdu,
3655 			   const u8 *da, const u8 *sa)
3656 {
3657 	struct ieee80211_local *local = sdata->local;
3658 	struct ieee80211_hdr *hdr = (void *)fast_tx->hdr;
3659 	struct ieee80211_tx_info *info;
3660 	struct ieee80211_tx_data tx;
3661 	ieee80211_tx_result r;
3662 	int hw_headroom = sdata->local->hw.extra_tx_headroom;
3663 	int extra_head = fast_tx->hdr_len - (ETH_HLEN - 2);
3664 
3665 	skb = skb_share_check(skb, GFP_ATOMIC);
3666 	if (unlikely(!skb))
3667 		return;
3668 
3669 	if ((hdr->frame_control & cpu_to_le16(IEEE80211_STYPE_QOS_DATA)) &&
3670 	    ieee80211_amsdu_aggregate(sdata, sta, fast_tx, skb, da, sa))
3671 		return;
3672 
3673 	/* will not be crypto-handled beyond what we do here, so use false
3674 	 * as the may-encrypt argument for the resize to not account for
3675 	 * more room than we already have in 'extra_head'
3676 	 */
3677 	if (unlikely(ieee80211_skb_resize(sdata, skb,
3678 					  max_t(int, extra_head + hw_headroom -
3679 						     skb_headroom(skb), 0),
3680 					  ENCRYPT_NO)))
3681 		goto free;
3682 
3683 	hdr = skb_push(skb, extra_head);
3684 	memcpy(skb->data, fast_tx->hdr, fast_tx->hdr_len);
3685 	memcpy(skb->data + fast_tx->da_offs, da, ETH_ALEN);
3686 	memcpy(skb->data + fast_tx->sa_offs, sa, ETH_ALEN);
3687 
3688 	info = IEEE80211_SKB_CB(skb);
3689 	memset(info, 0, sizeof(*info));
3690 	info->band = fast_tx->band;
3691 	info->control.vif = &sdata->vif;
3692 	info->flags = IEEE80211_TX_CTL_FIRST_FRAGMENT |
3693 		      IEEE80211_TX_CTL_DONTFRAG;
3694 	info->control.flags = IEEE80211_TX_CTRL_FAST_XMIT |
3695 			      u32_encode_bits(IEEE80211_LINK_UNSPECIFIED,
3696 					      IEEE80211_TX_CTRL_MLO_LINK);
3697 
3698 #ifdef CONFIG_MAC80211_DEBUGFS
3699 	if (local->force_tx_status)
3700 		info->flags |= IEEE80211_TX_CTL_REQ_TX_STATUS;
3701 #endif
3702 
3703 	if (hdr->frame_control & cpu_to_le16(IEEE80211_STYPE_QOS_DATA)) {
3704 		u8 tid = skb->priority & IEEE80211_QOS_CTL_TAG1D_MASK;
3705 
3706 		*ieee80211_get_qos_ctl(hdr) = tid;
3707 	}
3708 
3709 	__skb_queue_head_init(&tx.skbs);
3710 
3711 	tx.flags = IEEE80211_TX_UNICAST;
3712 	tx.local = local;
3713 	tx.sdata = sdata;
3714 	tx.sta = sta;
3715 	tx.key = fast_tx->key;
3716 
3717 	if (ieee80211_queue_skb(local, sdata, sta, skb))
3718 		return;
3719 
3720 	tx.skb = skb;
3721 	r = ieee80211_xmit_fast_finish(sdata, sta, fast_tx->pn_offs,
3722 				       fast_tx->key, &tx);
3723 	tx.skb = NULL;
3724 	if (r == TX_DROP)
3725 		goto free;
3726 
3727 	if (sdata->vif.type == NL80211_IFTYPE_AP_VLAN)
3728 		sdata = container_of(sdata->bss,
3729 				     struct ieee80211_sub_if_data, u.ap);
3730 
3731 	__skb_queue_tail(&tx.skbs, skb);
3732 	ieee80211_tx_frags(local, &sdata->vif, sta, &tx.skbs, false);
3733 	return;
3734 
3735 free:
3736 	kfree_skb(skb);
3737 }
3738 
ieee80211_xmit_fast(struct ieee80211_sub_if_data * sdata,struct sta_info * sta,struct ieee80211_fast_tx * fast_tx,struct sk_buff * skb)3739 static bool ieee80211_xmit_fast(struct ieee80211_sub_if_data *sdata,
3740 				struct sta_info *sta,
3741 				struct ieee80211_fast_tx *fast_tx,
3742 				struct sk_buff *skb)
3743 {
3744 	u16 ethertype = (skb->data[12] << 8) | skb->data[13];
3745 	struct ieee80211_hdr *hdr = (void *)fast_tx->hdr;
3746 	struct tid_ampdu_tx *tid_tx = NULL;
3747 	struct sk_buff *next;
3748 	struct ethhdr eth;
3749 	u8 tid = IEEE80211_NUM_TIDS;
3750 
3751 	/* control port protocol needs a lot of special handling */
3752 	if (cpu_to_be16(ethertype) == sdata->control_port_protocol)
3753 		return false;
3754 
3755 	/* only RFC 1042 SNAP */
3756 	if (ethertype < ETH_P_802_3_MIN)
3757 		return false;
3758 
3759 	/* don't handle TX status request here either */
3760 	if (skb->sk && skb_shinfo(skb)->tx_flags & SKBTX_WIFI_STATUS)
3761 		return false;
3762 
3763 	if (hdr->frame_control & cpu_to_le16(IEEE80211_STYPE_QOS_DATA)) {
3764 		tid = skb->priority & IEEE80211_QOS_CTL_TAG1D_MASK;
3765 		tid_tx = rcu_dereference(sta->ampdu_mlme.tid_tx[tid]);
3766 		if (tid_tx) {
3767 			if (!test_bit(HT_AGG_STATE_OPERATIONAL, &tid_tx->state))
3768 				return false;
3769 			if (tid_tx->timeout)
3770 				tid_tx->last_tx = jiffies;
3771 		}
3772 	}
3773 
3774 	memcpy(&eth, skb->data, ETH_HLEN - 2);
3775 
3776 	/* after this point (skb is modified) we cannot return false */
3777 	skb = ieee80211_tx_skb_fixup(skb, ieee80211_sdata_netdev_features(sdata));
3778 	if (!skb)
3779 		return true;
3780 
3781 	skb_list_walk_safe(skb, skb, next) {
3782 		skb_mark_not_on_list(skb);
3783 		__ieee80211_xmit_fast(sdata, sta, fast_tx, skb, tid_tx,
3784 				      eth.h_dest, eth.h_source);
3785 	}
3786 
3787 	return true;
3788 }
3789 
ieee80211_tx_dequeue(struct ieee80211_hw * hw,struct ieee80211_txq * txq)3790 struct sk_buff *ieee80211_tx_dequeue(struct ieee80211_hw *hw,
3791 				     struct ieee80211_txq *txq)
3792 {
3793 	struct ieee80211_local *local = hw_to_local(hw);
3794 	struct txq_info *txqi = container_of(txq, struct txq_info, txq);
3795 	struct ieee80211_hdr *hdr;
3796 	struct sk_buff *skb = NULL;
3797 	struct fq *fq = &local->fq;
3798 	struct fq_tin *tin = &txqi->tin;
3799 	struct ieee80211_tx_info *info;
3800 	struct ieee80211_tx_data tx;
3801 	ieee80211_tx_result r;
3802 	struct ieee80211_vif *vif = txq->vif;
3803 	int q = vif->hw_queue[txq->ac];
3804 	unsigned long flags;
3805 	bool q_stopped;
3806 
3807 	WARN_ON_ONCE(softirq_count() == 0);
3808 
3809 	if (!ieee80211_txq_airtime_check(hw, txq))
3810 		return NULL;
3811 
3812 begin:
3813 	spin_lock_irqsave(&local->queue_stop_reason_lock, flags);
3814 	q_stopped = local->queue_stop_reasons[q];
3815 	spin_unlock_irqrestore(&local->queue_stop_reason_lock, flags);
3816 
3817 	if (unlikely(q_stopped)) {
3818 		/* mark for waking later */
3819 		set_bit(IEEE80211_TXQ_DIRTY, &txqi->flags);
3820 		return NULL;
3821 	}
3822 
3823 	spin_lock_bh(&fq->lock);
3824 
3825 	/* Make sure fragments stay together. */
3826 	skb = __skb_dequeue(&txqi->frags);
3827 	if (unlikely(skb)) {
3828 		if (!(IEEE80211_SKB_CB(skb)->control.flags &
3829 				IEEE80211_TX_INTCFL_NEED_TXPROCESSING))
3830 			goto out;
3831 		IEEE80211_SKB_CB(skb)->control.flags &=
3832 			~IEEE80211_TX_INTCFL_NEED_TXPROCESSING;
3833 	} else {
3834 		if (unlikely(test_bit(IEEE80211_TXQ_STOP, &txqi->flags)))
3835 			goto out;
3836 
3837 		skb = fq_tin_dequeue(fq, tin, fq_tin_dequeue_func);
3838 	}
3839 
3840 	if (!skb)
3841 		goto out;
3842 
3843 	spin_unlock_bh(&fq->lock);
3844 
3845 	hdr = (struct ieee80211_hdr *)skb->data;
3846 	info = IEEE80211_SKB_CB(skb);
3847 
3848 	memset(&tx, 0, sizeof(tx));
3849 	__skb_queue_head_init(&tx.skbs);
3850 	tx.local = local;
3851 	tx.skb = skb;
3852 	tx.sdata = vif_to_sdata(info->control.vif);
3853 
3854 	if (txq->sta) {
3855 		tx.sta = container_of(txq->sta, struct sta_info, sta);
3856 		/*
3857 		 * Drop unicast frames to unauthorised stations unless they are
3858 		 * injected frames or EAPOL frames from the local station.
3859 		 */
3860 		if (unlikely(!(info->flags & IEEE80211_TX_CTL_INJECTED) &&
3861 			     ieee80211_is_data(hdr->frame_control) &&
3862 			     !ieee80211_vif_is_mesh(&tx.sdata->vif) &&
3863 			     tx.sdata->vif.type != NL80211_IFTYPE_OCB &&
3864 			     !is_multicast_ether_addr(hdr->addr1) &&
3865 			     !test_sta_flag(tx.sta, WLAN_STA_AUTHORIZED) &&
3866 			     (!(info->control.flags &
3867 				IEEE80211_TX_CTRL_PORT_CTRL_PROTO) ||
3868 			      !ieee80211_is_our_addr(tx.sdata, hdr->addr2,
3869 						     NULL)))) {
3870 			I802_DEBUG_INC(local->tx_handlers_drop_unauth_port);
3871 			ieee80211_free_txskb(&local->hw, skb);
3872 			goto begin;
3873 		}
3874 	}
3875 
3876 	/*
3877 	 * The key can be removed while the packet was queued, so need to call
3878 	 * this here to get the current key.
3879 	 */
3880 	r = ieee80211_tx_h_select_key(&tx);
3881 	if (r != TX_CONTINUE) {
3882 		ieee80211_free_txskb(&local->hw, skb);
3883 		goto begin;
3884 	}
3885 
3886 	if (test_bit(IEEE80211_TXQ_AMPDU, &txqi->flags))
3887 		info->flags |= (IEEE80211_TX_CTL_AMPDU |
3888 				IEEE80211_TX_CTL_DONTFRAG);
3889 
3890 	if (info->flags & IEEE80211_TX_CTL_HW_80211_ENCAP) {
3891 		if (!ieee80211_hw_check(&local->hw, HAS_RATE_CONTROL)) {
3892 			r = ieee80211_tx_h_rate_ctrl(&tx);
3893 			if (r != TX_CONTINUE) {
3894 				ieee80211_free_txskb(&local->hw, skb);
3895 				goto begin;
3896 			}
3897 		}
3898 		goto encap_out;
3899 	}
3900 
3901 	if (info->control.flags & IEEE80211_TX_CTRL_FAST_XMIT) {
3902 		struct sta_info *sta = container_of(txq->sta, struct sta_info,
3903 						    sta);
3904 		u8 pn_offs = 0;
3905 
3906 		if (tx.key &&
3907 		    (tx.key->conf.flags & IEEE80211_KEY_FLAG_GENERATE_IV))
3908 			pn_offs = ieee80211_hdrlen(hdr->frame_control);
3909 
3910 		r = ieee80211_xmit_fast_finish(sta->sdata, sta, pn_offs,
3911 					       tx.key, &tx);
3912 		if (r != TX_CONTINUE) {
3913 			ieee80211_free_txskb(&local->hw, skb);
3914 			goto begin;
3915 		}
3916 	} else {
3917 		if (invoke_tx_handlers_late(&tx))
3918 			goto begin;
3919 
3920 		skb = __skb_dequeue(&tx.skbs);
3921 		info = IEEE80211_SKB_CB(skb);
3922 
3923 		if (!skb_queue_empty(&tx.skbs)) {
3924 			spin_lock_bh(&fq->lock);
3925 			skb_queue_splice_tail(&tx.skbs, &txqi->frags);
3926 			spin_unlock_bh(&fq->lock);
3927 		}
3928 	}
3929 
3930 	if (skb_has_frag_list(skb) &&
3931 	    !ieee80211_hw_check(&local->hw, TX_FRAG_LIST)) {
3932 		if (skb_linearize(skb)) {
3933 			ieee80211_free_txskb(&local->hw, skb);
3934 			goto begin;
3935 		}
3936 	}
3937 
3938 	switch (tx.sdata->vif.type) {
3939 	case NL80211_IFTYPE_MONITOR:
3940 		if (tx.sdata->u.mntr.flags & MONITOR_FLAG_ACTIVE) {
3941 			vif = &tx.sdata->vif;
3942 			break;
3943 		}
3944 		tx.sdata = rcu_dereference(local->monitor_sdata);
3945 		if (tx.sdata) {
3946 			vif = &tx.sdata->vif;
3947 			info->hw_queue =
3948 				vif->hw_queue[skb_get_queue_mapping(skb)];
3949 		} else if (ieee80211_hw_check(&local->hw, QUEUE_CONTROL)) {
3950 			ieee80211_free_txskb(&local->hw, skb);
3951 			goto begin;
3952 		} else {
3953 			vif = NULL;
3954 		}
3955 		break;
3956 	case NL80211_IFTYPE_AP_VLAN:
3957 		tx.sdata = container_of(tx.sdata->bss,
3958 					struct ieee80211_sub_if_data, u.ap);
3959 		fallthrough;
3960 	default:
3961 		vif = &tx.sdata->vif;
3962 		break;
3963 	}
3964 
3965 encap_out:
3966 	info->control.vif = vif;
3967 
3968 	if (tx.sta &&
3969 	    wiphy_ext_feature_isset(local->hw.wiphy, NL80211_EXT_FEATURE_AQL)) {
3970 		bool ampdu = txq->ac != IEEE80211_AC_VO;
3971 		u32 airtime;
3972 
3973 		airtime = ieee80211_calc_expected_tx_airtime(hw, vif, txq->sta,
3974 							     skb->len, ampdu);
3975 		if (airtime) {
3976 			airtime = ieee80211_info_set_tx_time_est(info, airtime);
3977 			ieee80211_sta_update_pending_airtime(local, tx.sta,
3978 							     txq->ac,
3979 							     airtime,
3980 							     false);
3981 		}
3982 	}
3983 
3984 	return skb;
3985 
3986 out:
3987 	spin_unlock_bh(&fq->lock);
3988 
3989 	return skb;
3990 }
3991 EXPORT_SYMBOL(ieee80211_tx_dequeue);
3992 
ieee80211_sta_deficit(struct sta_info * sta,u8 ac)3993 static inline s32 ieee80211_sta_deficit(struct sta_info *sta, u8 ac)
3994 {
3995 	struct airtime_info *air_info = &sta->airtime[ac];
3996 
3997 	return air_info->deficit - atomic_read(&air_info->aql_tx_pending);
3998 }
3999 
4000 static void
ieee80211_txq_set_active(struct txq_info * txqi)4001 ieee80211_txq_set_active(struct txq_info *txqi)
4002 {
4003 	struct sta_info *sta;
4004 
4005 	if (!txqi->txq.sta)
4006 		return;
4007 
4008 	sta = container_of(txqi->txq.sta, struct sta_info, sta);
4009 	sta->airtime[txqi->txq.ac].last_active = (u32)jiffies;
4010 }
4011 
4012 static bool
ieee80211_txq_keep_active(struct txq_info * txqi)4013 ieee80211_txq_keep_active(struct txq_info *txqi)
4014 {
4015 	struct sta_info *sta;
4016 	u32 diff;
4017 
4018 	if (!txqi->txq.sta)
4019 		return false;
4020 
4021 	sta = container_of(txqi->txq.sta, struct sta_info, sta);
4022 	if (ieee80211_sta_deficit(sta, txqi->txq.ac) >= 0)
4023 		return false;
4024 
4025 	diff = (u32)jiffies - sta->airtime[txqi->txq.ac].last_active;
4026 
4027 	return diff <= AIRTIME_ACTIVE_DURATION;
4028 }
4029 
ieee80211_next_txq(struct ieee80211_hw * hw,u8 ac)4030 struct ieee80211_txq *ieee80211_next_txq(struct ieee80211_hw *hw, u8 ac)
4031 {
4032 	struct ieee80211_local *local = hw_to_local(hw);
4033 	struct ieee80211_txq *ret = NULL;
4034 	struct txq_info *txqi = NULL, *head = NULL;
4035 	bool found_eligible_txq = false;
4036 
4037 	spin_lock_bh(&local->active_txq_lock[ac]);
4038 
4039 	if (!local->schedule_round[ac])
4040 		goto out;
4041 
4042  begin:
4043 	txqi = list_first_entry_or_null(&local->active_txqs[ac],
4044 					struct txq_info,
4045 					schedule_order);
4046 	if (!txqi)
4047 		goto out;
4048 
4049 	if (txqi == head) {
4050 		if (!found_eligible_txq)
4051 			goto out;
4052 		else
4053 			found_eligible_txq = false;
4054 	}
4055 
4056 	if (!head)
4057 		head = txqi;
4058 
4059 	if (txqi->txq.sta) {
4060 		struct sta_info *sta = container_of(txqi->txq.sta,
4061 						    struct sta_info, sta);
4062 		bool aql_check = ieee80211_txq_airtime_check(hw, &txqi->txq);
4063 		s32 deficit = ieee80211_sta_deficit(sta, txqi->txq.ac);
4064 
4065 		if (aql_check)
4066 			found_eligible_txq = true;
4067 
4068 		if (deficit < 0)
4069 			sta->airtime[txqi->txq.ac].deficit +=
4070 				sta->airtime_weight;
4071 
4072 		if (deficit < 0 || !aql_check) {
4073 			list_move_tail(&txqi->schedule_order,
4074 				       &local->active_txqs[txqi->txq.ac]);
4075 			goto begin;
4076 		}
4077 	}
4078 
4079 	if (txqi->schedule_round == local->schedule_round[ac])
4080 		goto out;
4081 
4082 	list_del_init(&txqi->schedule_order);
4083 	txqi->schedule_round = local->schedule_round[ac];
4084 	ret = &txqi->txq;
4085 
4086 out:
4087 	spin_unlock_bh(&local->active_txq_lock[ac]);
4088 	return ret;
4089 }
4090 EXPORT_SYMBOL(ieee80211_next_txq);
4091 
__ieee80211_schedule_txq(struct ieee80211_hw * hw,struct ieee80211_txq * txq,bool force)4092 void __ieee80211_schedule_txq(struct ieee80211_hw *hw,
4093 			      struct ieee80211_txq *txq,
4094 			      bool force)
4095 {
4096 	struct ieee80211_local *local = hw_to_local(hw);
4097 	struct txq_info *txqi = to_txq_info(txq);
4098 	bool has_queue;
4099 
4100 	spin_lock_bh(&local->active_txq_lock[txq->ac]);
4101 
4102 	has_queue = force || txq_has_queue(txq);
4103 	if (list_empty(&txqi->schedule_order) &&
4104 	    (has_queue || ieee80211_txq_keep_active(txqi))) {
4105 		/* If airtime accounting is active, always enqueue STAs at the
4106 		 * head of the list to ensure that they only get moved to the
4107 		 * back by the airtime DRR scheduler once they have a negative
4108 		 * deficit. A station that already has a negative deficit will
4109 		 * get immediately moved to the back of the list on the next
4110 		 * call to ieee80211_next_txq().
4111 		 */
4112 		if (txqi->txq.sta && local->airtime_flags && has_queue &&
4113 		    wiphy_ext_feature_isset(local->hw.wiphy,
4114 					    NL80211_EXT_FEATURE_AIRTIME_FAIRNESS))
4115 			list_add(&txqi->schedule_order,
4116 				 &local->active_txqs[txq->ac]);
4117 		else
4118 			list_add_tail(&txqi->schedule_order,
4119 				      &local->active_txqs[txq->ac]);
4120 		if (has_queue)
4121 			ieee80211_txq_set_active(txqi);
4122 	}
4123 
4124 	spin_unlock_bh(&local->active_txq_lock[txq->ac]);
4125 }
4126 EXPORT_SYMBOL(__ieee80211_schedule_txq);
4127 
4128 DEFINE_STATIC_KEY_FALSE(aql_disable);
4129 
ieee80211_txq_airtime_check(struct ieee80211_hw * hw,struct ieee80211_txq * txq)4130 bool ieee80211_txq_airtime_check(struct ieee80211_hw *hw,
4131 				 struct ieee80211_txq *txq)
4132 {
4133 	struct sta_info *sta;
4134 	struct ieee80211_local *local = hw_to_local(hw);
4135 
4136 	if (!wiphy_ext_feature_isset(local->hw.wiphy, NL80211_EXT_FEATURE_AQL))
4137 		return true;
4138 
4139 	if (static_branch_unlikely(&aql_disable))
4140 		return true;
4141 
4142 	if (!txq->sta)
4143 		return true;
4144 
4145 	if (unlikely(txq->tid == IEEE80211_NUM_TIDS))
4146 		return true;
4147 
4148 	sta = container_of(txq->sta, struct sta_info, sta);
4149 	if (atomic_read(&sta->airtime[txq->ac].aql_tx_pending) <
4150 	    sta->airtime[txq->ac].aql_limit_low)
4151 		return true;
4152 
4153 	if (atomic_read(&local->aql_total_pending_airtime) <
4154 	    local->aql_threshold &&
4155 	    atomic_read(&sta->airtime[txq->ac].aql_tx_pending) <
4156 	    sta->airtime[txq->ac].aql_limit_high)
4157 		return true;
4158 
4159 	return false;
4160 }
4161 EXPORT_SYMBOL(ieee80211_txq_airtime_check);
4162 
4163 static bool
ieee80211_txq_schedule_airtime_check(struct ieee80211_local * local,u8 ac)4164 ieee80211_txq_schedule_airtime_check(struct ieee80211_local *local, u8 ac)
4165 {
4166 	unsigned int num_txq = 0;
4167 	struct txq_info *txq;
4168 	u32 aql_limit;
4169 
4170 	if (!wiphy_ext_feature_isset(local->hw.wiphy, NL80211_EXT_FEATURE_AQL))
4171 		return true;
4172 
4173 	list_for_each_entry(txq, &local->active_txqs[ac], schedule_order)
4174 		num_txq++;
4175 
4176 	aql_limit = (num_txq - 1) * local->aql_txq_limit_low[ac] / 2 +
4177 		    local->aql_txq_limit_high[ac];
4178 
4179 	return atomic_read(&local->aql_ac_pending_airtime[ac]) < aql_limit;
4180 }
4181 
ieee80211_txq_may_transmit(struct ieee80211_hw * hw,struct ieee80211_txq * txq)4182 bool ieee80211_txq_may_transmit(struct ieee80211_hw *hw,
4183 				struct ieee80211_txq *txq)
4184 {
4185 	struct ieee80211_local *local = hw_to_local(hw);
4186 	struct txq_info *iter, *tmp, *txqi = to_txq_info(txq);
4187 	struct sta_info *sta;
4188 	u8 ac = txq->ac;
4189 
4190 	spin_lock_bh(&local->active_txq_lock[ac]);
4191 
4192 	if (!txqi->txq.sta)
4193 		goto out;
4194 
4195 	if (list_empty(&txqi->schedule_order))
4196 		goto out;
4197 
4198 	if (!ieee80211_txq_schedule_airtime_check(local, ac))
4199 		goto out;
4200 
4201 	list_for_each_entry_safe(iter, tmp, &local->active_txqs[ac],
4202 				 schedule_order) {
4203 		if (iter == txqi)
4204 			break;
4205 
4206 		if (!iter->txq.sta) {
4207 			list_move_tail(&iter->schedule_order,
4208 				       &local->active_txqs[ac]);
4209 			continue;
4210 		}
4211 		sta = container_of(iter->txq.sta, struct sta_info, sta);
4212 		if (ieee80211_sta_deficit(sta, ac) < 0)
4213 			sta->airtime[ac].deficit += sta->airtime_weight;
4214 		list_move_tail(&iter->schedule_order, &local->active_txqs[ac]);
4215 	}
4216 
4217 	sta = container_of(txqi->txq.sta, struct sta_info, sta);
4218 	if (sta->airtime[ac].deficit >= 0)
4219 		goto out;
4220 
4221 	sta->airtime[ac].deficit += sta->airtime_weight;
4222 	list_move_tail(&txqi->schedule_order, &local->active_txqs[ac]);
4223 	spin_unlock_bh(&local->active_txq_lock[ac]);
4224 
4225 	return false;
4226 out:
4227 	if (!list_empty(&txqi->schedule_order))
4228 		list_del_init(&txqi->schedule_order);
4229 	spin_unlock_bh(&local->active_txq_lock[ac]);
4230 
4231 	return true;
4232 }
4233 EXPORT_SYMBOL(ieee80211_txq_may_transmit);
4234 
ieee80211_txq_schedule_start(struct ieee80211_hw * hw,u8 ac)4235 void ieee80211_txq_schedule_start(struct ieee80211_hw *hw, u8 ac)
4236 {
4237 	struct ieee80211_local *local = hw_to_local(hw);
4238 
4239 	spin_lock_bh(&local->active_txq_lock[ac]);
4240 
4241 	if (ieee80211_txq_schedule_airtime_check(local, ac)) {
4242 		local->schedule_round[ac]++;
4243 		if (!local->schedule_round[ac])
4244 			local->schedule_round[ac]++;
4245 	} else {
4246 		local->schedule_round[ac] = 0;
4247 	}
4248 
4249 	spin_unlock_bh(&local->active_txq_lock[ac]);
4250 }
4251 EXPORT_SYMBOL(ieee80211_txq_schedule_start);
4252 
__ieee80211_subif_start_xmit(struct sk_buff * skb,struct net_device * dev,u32 info_flags,u32 ctrl_flags,u64 * cookie)4253 void __ieee80211_subif_start_xmit(struct sk_buff *skb,
4254 				  struct net_device *dev,
4255 				  u32 info_flags,
4256 				  u32 ctrl_flags,
4257 				  u64 *cookie)
4258 {
4259 	struct ieee80211_sub_if_data *sdata = IEEE80211_DEV_TO_SUB_IF(dev);
4260 	struct ieee80211_local *local = sdata->local;
4261 	struct sta_info *sta;
4262 	struct sk_buff *next;
4263 	int len = skb->len;
4264 
4265 	if (unlikely(!ieee80211_sdata_running(sdata) || skb->len < ETH_HLEN)) {
4266 		kfree_skb(skb);
4267 		return;
4268 	}
4269 
4270 	sk_pacing_shift_update(skb->sk, sdata->local->hw.tx_sk_pacing_shift);
4271 
4272 	rcu_read_lock();
4273 
4274 	if (ieee80211_vif_is_mesh(&sdata->vif) &&
4275 	    ieee80211_hw_check(&local->hw, SUPPORT_FAST_XMIT) &&
4276 	    ieee80211_mesh_xmit_fast(sdata, skb, ctrl_flags))
4277 		goto out;
4278 
4279 	if (ieee80211_lookup_ra_sta(sdata, skb, &sta))
4280 		goto out_free;
4281 
4282 	if (IS_ERR(sta))
4283 		sta = NULL;
4284 
4285 	skb_set_queue_mapping(skb, ieee80211_select_queue(sdata, sta, skb));
4286 	ieee80211_aggr_check(sdata, sta, skb);
4287 
4288 	if (sta) {
4289 		struct ieee80211_fast_tx *fast_tx;
4290 
4291 		fast_tx = rcu_dereference(sta->fast_tx);
4292 
4293 		if (fast_tx &&
4294 		    ieee80211_xmit_fast(sdata, sta, fast_tx, skb))
4295 			goto out;
4296 	}
4297 
4298 	/* the frame could be fragmented, software-encrypted, and other
4299 	 * things so we cannot really handle checksum or GSO offload.
4300 	 * fix it up in software before we handle anything else.
4301 	 */
4302 	skb = ieee80211_tx_skb_fixup(skb, 0);
4303 	if (!skb) {
4304 		len = 0;
4305 		goto out;
4306 	}
4307 
4308 	skb_list_walk_safe(skb, skb, next) {
4309 		skb_mark_not_on_list(skb);
4310 
4311 		if (skb->protocol == sdata->control_port_protocol)
4312 			ctrl_flags |= IEEE80211_TX_CTRL_SKIP_MPATH_LOOKUP;
4313 
4314 		skb = ieee80211_build_hdr(sdata, skb, info_flags,
4315 					  sta, ctrl_flags, cookie);
4316 		if (IS_ERR(skb)) {
4317 			kfree_skb_list(next);
4318 			goto out;
4319 		}
4320 
4321 		dev_sw_netstats_tx_add(dev, 1, skb->len);
4322 
4323 		ieee80211_xmit(sdata, sta, skb);
4324 	}
4325 	goto out;
4326  out_free:
4327 	kfree_skb(skb);
4328 	len = 0;
4329  out:
4330 	if (len)
4331 		ieee80211_tpt_led_trig_tx(local, len);
4332 	rcu_read_unlock();
4333 }
4334 
ieee80211_change_da(struct sk_buff * skb,struct sta_info * sta)4335 static int ieee80211_change_da(struct sk_buff *skb, struct sta_info *sta)
4336 {
4337 	struct ethhdr *eth;
4338 	int err;
4339 
4340 	err = skb_ensure_writable(skb, ETH_HLEN);
4341 	if (unlikely(err))
4342 		return err;
4343 
4344 	eth = (void *)skb->data;
4345 	ether_addr_copy(eth->h_dest, sta->sta.addr);
4346 
4347 	return 0;
4348 }
4349 
ieee80211_multicast_to_unicast(struct sk_buff * skb,struct net_device * dev)4350 static bool ieee80211_multicast_to_unicast(struct sk_buff *skb,
4351 					   struct net_device *dev)
4352 {
4353 	struct ieee80211_sub_if_data *sdata = IEEE80211_DEV_TO_SUB_IF(dev);
4354 	const struct ethhdr *eth = (void *)skb->data;
4355 	const struct vlan_ethhdr *ethvlan = (void *)skb->data;
4356 	__be16 ethertype;
4357 
4358 	switch (sdata->vif.type) {
4359 	case NL80211_IFTYPE_AP_VLAN:
4360 		if (sdata->u.vlan.sta)
4361 			return false;
4362 		if (sdata->wdev.use_4addr)
4363 			return false;
4364 		fallthrough;
4365 	case NL80211_IFTYPE_AP:
4366 		/* check runtime toggle for this bss */
4367 		if (!sdata->bss->multicast_to_unicast)
4368 			return false;
4369 		break;
4370 	default:
4371 		return false;
4372 	}
4373 
4374 	/* multicast to unicast conversion only for some payload */
4375 	ethertype = eth->h_proto;
4376 	if (ethertype == htons(ETH_P_8021Q) && skb->len >= VLAN_ETH_HLEN)
4377 		ethertype = ethvlan->h_vlan_encapsulated_proto;
4378 	switch (ethertype) {
4379 	case htons(ETH_P_ARP):
4380 	case htons(ETH_P_IP):
4381 	case htons(ETH_P_IPV6):
4382 		break;
4383 	default:
4384 		return false;
4385 	}
4386 
4387 	return true;
4388 }
4389 
4390 static void
ieee80211_convert_to_unicast(struct sk_buff * skb,struct net_device * dev,struct sk_buff_head * queue)4391 ieee80211_convert_to_unicast(struct sk_buff *skb, struct net_device *dev,
4392 			     struct sk_buff_head *queue)
4393 {
4394 	struct ieee80211_sub_if_data *sdata = IEEE80211_DEV_TO_SUB_IF(dev);
4395 	struct ieee80211_local *local = sdata->local;
4396 	const struct ethhdr *eth = (struct ethhdr *)skb->data;
4397 	struct sta_info *sta, *first = NULL;
4398 	struct sk_buff *cloned_skb;
4399 
4400 	rcu_read_lock();
4401 
4402 	list_for_each_entry_rcu(sta, &local->sta_list, list) {
4403 		if (sdata != sta->sdata)
4404 			/* AP-VLAN mismatch */
4405 			continue;
4406 		if (unlikely(ether_addr_equal(eth->h_source, sta->sta.addr)))
4407 			/* do not send back to source */
4408 			continue;
4409 		if (!first) {
4410 			first = sta;
4411 			continue;
4412 		}
4413 		cloned_skb = skb_clone(skb, GFP_ATOMIC);
4414 		if (!cloned_skb)
4415 			goto multicast;
4416 		if (unlikely(ieee80211_change_da(cloned_skb, sta))) {
4417 			dev_kfree_skb(cloned_skb);
4418 			goto multicast;
4419 		}
4420 		__skb_queue_tail(queue, cloned_skb);
4421 	}
4422 
4423 	if (likely(first)) {
4424 		if (unlikely(ieee80211_change_da(skb, first)))
4425 			goto multicast;
4426 		__skb_queue_tail(queue, skb);
4427 	} else {
4428 		/* no STA connected, drop */
4429 		kfree_skb(skb);
4430 		skb = NULL;
4431 	}
4432 
4433 	goto out;
4434 multicast:
4435 	__skb_queue_purge(queue);
4436 	__skb_queue_tail(queue, skb);
4437 out:
4438 	rcu_read_unlock();
4439 }
4440 
ieee80211_mlo_multicast_tx_one(struct ieee80211_sub_if_data * sdata,struct sk_buff * skb,u32 ctrl_flags,unsigned int link_id)4441 static void ieee80211_mlo_multicast_tx_one(struct ieee80211_sub_if_data *sdata,
4442 					   struct sk_buff *skb, u32 ctrl_flags,
4443 					   unsigned int link_id)
4444 {
4445 	struct sk_buff *out;
4446 
4447 	out = skb_copy(skb, GFP_ATOMIC);
4448 	if (!out)
4449 		return;
4450 
4451 	ctrl_flags |= u32_encode_bits(link_id, IEEE80211_TX_CTRL_MLO_LINK);
4452 	__ieee80211_subif_start_xmit(out, sdata->dev, 0, ctrl_flags, NULL);
4453 }
4454 
ieee80211_mlo_multicast_tx(struct net_device * dev,struct sk_buff * skb)4455 static void ieee80211_mlo_multicast_tx(struct net_device *dev,
4456 				       struct sk_buff *skb)
4457 {
4458 	struct ieee80211_sub_if_data *sdata = IEEE80211_DEV_TO_SUB_IF(dev);
4459 	unsigned long links = sdata->vif.active_links;
4460 	unsigned int link;
4461 	u32 ctrl_flags = IEEE80211_TX_CTRL_MCAST_MLO_FIRST_TX;
4462 
4463 	if (hweight16(links) == 1) {
4464 		ctrl_flags |= u32_encode_bits(__ffs(links),
4465 					      IEEE80211_TX_CTRL_MLO_LINK);
4466 
4467 		__ieee80211_subif_start_xmit(skb, sdata->dev, 0, ctrl_flags,
4468 					     NULL);
4469 		return;
4470 	}
4471 
4472 	for_each_set_bit(link, &links, IEEE80211_MLD_MAX_NUM_LINKS) {
4473 		ieee80211_mlo_multicast_tx_one(sdata, skb, ctrl_flags, link);
4474 		ctrl_flags = 0;
4475 	}
4476 	kfree_skb(skb);
4477 }
4478 
4479 /**
4480  * ieee80211_subif_start_xmit - netif start_xmit function for 802.3 vifs
4481  * @skb: packet to be sent
4482  * @dev: incoming interface
4483  *
4484  * On failure skb will be freed.
4485  */
ieee80211_subif_start_xmit(struct sk_buff * skb,struct net_device * dev)4486 netdev_tx_t ieee80211_subif_start_xmit(struct sk_buff *skb,
4487 				       struct net_device *dev)
4488 {
4489 	struct ieee80211_sub_if_data *sdata = IEEE80211_DEV_TO_SUB_IF(dev);
4490 	const struct ethhdr *eth = (void *)skb->data;
4491 
4492 	if (likely(!is_multicast_ether_addr(eth->h_dest)))
4493 		goto normal;
4494 
4495 	if (unlikely(!ieee80211_sdata_running(sdata))) {
4496 		kfree_skb(skb);
4497 		return NETDEV_TX_OK;
4498 	}
4499 
4500 	if (unlikely(ieee80211_multicast_to_unicast(skb, dev))) {
4501 		struct sk_buff_head queue;
4502 
4503 		__skb_queue_head_init(&queue);
4504 		ieee80211_convert_to_unicast(skb, dev, &queue);
4505 		while ((skb = __skb_dequeue(&queue)))
4506 			__ieee80211_subif_start_xmit(skb, dev, 0,
4507 						     IEEE80211_TX_CTRL_MLO_LINK_UNSPEC,
4508 						     NULL);
4509 	} else if (ieee80211_vif_is_mld(&sdata->vif) &&
4510 		   sdata->vif.type == NL80211_IFTYPE_AP &&
4511 		   !ieee80211_hw_check(&sdata->local->hw, MLO_MCAST_MULTI_LINK_TX)) {
4512 		ieee80211_mlo_multicast_tx(dev, skb);
4513 	} else {
4514 normal:
4515 		__ieee80211_subif_start_xmit(skb, dev, 0,
4516 					     IEEE80211_TX_CTRL_MLO_LINK_UNSPEC,
4517 					     NULL);
4518 	}
4519 
4520 	return NETDEV_TX_OK;
4521 }
4522 
4523 
4524 
__ieee80211_tx_8023(struct ieee80211_sub_if_data * sdata,struct sk_buff * skb,struct sta_info * sta,bool txpending)4525 static bool __ieee80211_tx_8023(struct ieee80211_sub_if_data *sdata,
4526 				struct sk_buff *skb, struct sta_info *sta,
4527 				bool txpending)
4528 {
4529 	struct ieee80211_local *local = sdata->local;
4530 	struct ieee80211_tx_control control = {};
4531 	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
4532 	struct ieee80211_sta *pubsta = NULL;
4533 	unsigned long flags;
4534 	int q = info->hw_queue;
4535 
4536 	spin_lock_irqsave(&local->queue_stop_reason_lock, flags);
4537 
4538 	if (local->queue_stop_reasons[q] ||
4539 	    (!txpending && !skb_queue_empty(&local->pending[q]))) {
4540 		if (txpending)
4541 			skb_queue_head(&local->pending[q], skb);
4542 		else
4543 			skb_queue_tail(&local->pending[q], skb);
4544 
4545 		spin_unlock_irqrestore(&local->queue_stop_reason_lock, flags);
4546 
4547 		return false;
4548 	}
4549 
4550 	spin_unlock_irqrestore(&local->queue_stop_reason_lock, flags);
4551 
4552 	if (sta && sta->uploaded)
4553 		pubsta = &sta->sta;
4554 
4555 	control.sta = pubsta;
4556 
4557 	drv_tx(local, &control, skb);
4558 
4559 	return true;
4560 }
4561 
ieee80211_tx_8023(struct ieee80211_sub_if_data * sdata,struct sk_buff * skb,struct sta_info * sta,bool txpending)4562 static bool ieee80211_tx_8023(struct ieee80211_sub_if_data *sdata,
4563 			      struct sk_buff *skb, struct sta_info *sta,
4564 			      bool txpending)
4565 {
4566 	struct ieee80211_local *local = sdata->local;
4567 	struct sk_buff *next;
4568 	bool ret = true;
4569 
4570 	if (ieee80211_queue_skb(local, sdata, sta, skb))
4571 		return true;
4572 
4573 	skb_list_walk_safe(skb, skb, next) {
4574 		skb_mark_not_on_list(skb);
4575 		if (!__ieee80211_tx_8023(sdata, skb, sta, txpending))
4576 			ret = false;
4577 	}
4578 
4579 	return ret;
4580 }
4581 
ieee80211_8023_xmit(struct ieee80211_sub_if_data * sdata,struct net_device * dev,struct sta_info * sta,struct ieee80211_key * key,struct sk_buff * skb)4582 static void ieee80211_8023_xmit(struct ieee80211_sub_if_data *sdata,
4583 				struct net_device *dev, struct sta_info *sta,
4584 				struct ieee80211_key *key, struct sk_buff *skb)
4585 {
4586 	struct ieee80211_tx_info *info;
4587 	struct ieee80211_local *local = sdata->local;
4588 	struct tid_ampdu_tx *tid_tx;
4589 	struct sk_buff *seg, *next;
4590 	unsigned int skbs = 0, len = 0;
4591 	u16 queue;
4592 	u8 tid;
4593 
4594 	queue = ieee80211_select_queue(sdata, sta, skb);
4595 	skb_set_queue_mapping(skb, queue);
4596 
4597 	if (unlikely(test_bit(SCAN_SW_SCANNING, &local->scanning)) &&
4598 	    test_bit(SDATA_STATE_OFFCHANNEL, &sdata->state))
4599 		goto out_free;
4600 
4601 	skb = skb_share_check(skb, GFP_ATOMIC);
4602 	if (unlikely(!skb))
4603 		return;
4604 
4605 	ieee80211_aggr_check(sdata, sta, skb);
4606 
4607 	tid = skb->priority & IEEE80211_QOS_CTL_TAG1D_MASK;
4608 	tid_tx = rcu_dereference(sta->ampdu_mlme.tid_tx[tid]);
4609 	if (tid_tx) {
4610 		if (!test_bit(HT_AGG_STATE_OPERATIONAL, &tid_tx->state)) {
4611 			/* fall back to non-offload slow path */
4612 			__ieee80211_subif_start_xmit(skb, dev, 0,
4613 						     IEEE80211_TX_CTRL_MLO_LINK_UNSPEC,
4614 						     NULL);
4615 			return;
4616 		}
4617 
4618 		if (tid_tx->timeout)
4619 			tid_tx->last_tx = jiffies;
4620 	}
4621 
4622 	skb = ieee80211_tx_skb_fixup(skb, ieee80211_sdata_netdev_features(sdata));
4623 	if (!skb)
4624 		return;
4625 
4626 	info = IEEE80211_SKB_CB(skb);
4627 	memset(info, 0, sizeof(*info));
4628 
4629 	info->hw_queue = sdata->vif.hw_queue[queue];
4630 
4631 	if (sdata->vif.type == NL80211_IFTYPE_AP_VLAN)
4632 		sdata = container_of(sdata->bss,
4633 				     struct ieee80211_sub_if_data, u.ap);
4634 
4635 	info->flags |= IEEE80211_TX_CTL_HW_80211_ENCAP;
4636 	info->control.vif = &sdata->vif;
4637 
4638 	if (key)
4639 		info->control.hw_key = &key->conf;
4640 
4641 	skb_list_walk_safe(skb, seg, next) {
4642 		skbs++;
4643 		len += seg->len;
4644 		if (seg != skb)
4645 			memcpy(IEEE80211_SKB_CB(seg), info, sizeof(*info));
4646 	}
4647 
4648 	if (unlikely(skb->sk &&
4649 		     skb_shinfo(skb)->tx_flags & SKBTX_WIFI_STATUS))
4650 		info->ack_frame_id = ieee80211_store_ack_skb(local, skb,
4651 							     &info->flags, NULL);
4652 
4653 	dev_sw_netstats_tx_add(dev, skbs, len);
4654 	sta->deflink.tx_stats.packets[queue] += skbs;
4655 	sta->deflink.tx_stats.bytes[queue] += len;
4656 
4657 	ieee80211_tpt_led_trig_tx(local, len);
4658 
4659 	ieee80211_tx_8023(sdata, skb, sta, false);
4660 
4661 	return;
4662 
4663 out_free:
4664 	kfree_skb(skb);
4665 }
4666 
ieee80211_subif_start_xmit_8023(struct sk_buff * skb,struct net_device * dev)4667 netdev_tx_t ieee80211_subif_start_xmit_8023(struct sk_buff *skb,
4668 					    struct net_device *dev)
4669 {
4670 	struct ieee80211_sub_if_data *sdata = IEEE80211_DEV_TO_SUB_IF(dev);
4671 	struct ethhdr *ehdr = (struct ethhdr *)skb->data;
4672 	struct ieee80211_key *key;
4673 	struct sta_info *sta;
4674 
4675 	if (unlikely(!ieee80211_sdata_running(sdata) || skb->len < ETH_HLEN)) {
4676 		kfree_skb(skb);
4677 		return NETDEV_TX_OK;
4678 	}
4679 
4680 	rcu_read_lock();
4681 
4682 	if (ieee80211_lookup_ra_sta(sdata, skb, &sta)) {
4683 		kfree_skb(skb);
4684 		goto out;
4685 	}
4686 
4687 	if (unlikely(IS_ERR_OR_NULL(sta) || !sta->uploaded ||
4688 	    !test_sta_flag(sta, WLAN_STA_AUTHORIZED) ||
4689 	    sdata->control_port_protocol == ehdr->h_proto))
4690 		goto skip_offload;
4691 
4692 	key = rcu_dereference(sta->ptk[sta->ptk_idx]);
4693 	if (!key)
4694 		key = rcu_dereference(sdata->default_unicast_key);
4695 
4696 	if (key && (!(key->flags & KEY_FLAG_UPLOADED_TO_HARDWARE) ||
4697 		    key->conf.cipher == WLAN_CIPHER_SUITE_TKIP))
4698 		goto skip_offload;
4699 
4700 	sk_pacing_shift_update(skb->sk, sdata->local->hw.tx_sk_pacing_shift);
4701 	ieee80211_8023_xmit(sdata, dev, sta, key, skb);
4702 	goto out;
4703 
4704 skip_offload:
4705 	ieee80211_subif_start_xmit(skb, dev);
4706 out:
4707 	rcu_read_unlock();
4708 
4709 	return NETDEV_TX_OK;
4710 }
4711 
4712 struct sk_buff *
ieee80211_build_data_template(struct ieee80211_sub_if_data * sdata,struct sk_buff * skb,u32 info_flags)4713 ieee80211_build_data_template(struct ieee80211_sub_if_data *sdata,
4714 			      struct sk_buff *skb, u32 info_flags)
4715 {
4716 	struct ieee80211_hdr *hdr;
4717 	struct ieee80211_tx_data tx = {
4718 		.local = sdata->local,
4719 		.sdata = sdata,
4720 	};
4721 	struct sta_info *sta;
4722 
4723 	rcu_read_lock();
4724 
4725 	if (ieee80211_lookup_ra_sta(sdata, skb, &sta)) {
4726 		kfree_skb(skb);
4727 		skb = ERR_PTR(-EINVAL);
4728 		goto out;
4729 	}
4730 
4731 	skb = ieee80211_build_hdr(sdata, skb, info_flags, sta,
4732 				  IEEE80211_TX_CTRL_MLO_LINK_UNSPEC, NULL);
4733 	if (IS_ERR(skb))
4734 		goto out;
4735 
4736 	hdr = (void *)skb->data;
4737 	tx.sta = sta_info_get(sdata, hdr->addr1);
4738 	tx.skb = skb;
4739 
4740 	if (ieee80211_tx_h_select_key(&tx) != TX_CONTINUE) {
4741 		rcu_read_unlock();
4742 		kfree_skb(skb);
4743 		return ERR_PTR(-EINVAL);
4744 	}
4745 
4746 out:
4747 	rcu_read_unlock();
4748 	return skb;
4749 }
4750 
4751 /*
4752  * ieee80211_clear_tx_pending may not be called in a context where
4753  * it is possible that it packets could come in again.
4754  */
ieee80211_clear_tx_pending(struct ieee80211_local * local)4755 void ieee80211_clear_tx_pending(struct ieee80211_local *local)
4756 {
4757 	struct sk_buff *skb;
4758 	int i;
4759 
4760 	for (i = 0; i < local->hw.queues; i++) {
4761 		while ((skb = skb_dequeue(&local->pending[i])) != NULL)
4762 			ieee80211_free_txskb(&local->hw, skb);
4763 	}
4764 }
4765 
4766 /*
4767  * Returns false if the frame couldn't be transmitted but was queued instead,
4768  * which in this case means re-queued -- take as an indication to stop sending
4769  * more pending frames.
4770  */
ieee80211_tx_pending_skb(struct ieee80211_local * local,struct sk_buff * skb)4771 static bool ieee80211_tx_pending_skb(struct ieee80211_local *local,
4772 				     struct sk_buff *skb)
4773 {
4774 	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
4775 	struct ieee80211_sub_if_data *sdata;
4776 	struct sta_info *sta;
4777 	struct ieee80211_hdr *hdr;
4778 	bool result;
4779 	struct ieee80211_chanctx_conf *chanctx_conf;
4780 
4781 	sdata = vif_to_sdata(info->control.vif);
4782 
4783 	if (info->control.flags & IEEE80211_TX_INTCFL_NEED_TXPROCESSING) {
4784 		/* update band only for non-MLD */
4785 		if (!ieee80211_vif_is_mld(&sdata->vif)) {
4786 			chanctx_conf =
4787 				rcu_dereference(sdata->vif.bss_conf.chanctx_conf);
4788 			if (unlikely(!chanctx_conf)) {
4789 				dev_kfree_skb(skb);
4790 				return true;
4791 			}
4792 			info->band = chanctx_conf->def.chan->band;
4793 		}
4794 		result = ieee80211_tx(sdata, NULL, skb, true);
4795 	} else if (info->flags & IEEE80211_TX_CTL_HW_80211_ENCAP) {
4796 		if (ieee80211_lookup_ra_sta(sdata, skb, &sta)) {
4797 			dev_kfree_skb(skb);
4798 			return true;
4799 		}
4800 
4801 		if (IS_ERR(sta) || (sta && !sta->uploaded))
4802 			sta = NULL;
4803 
4804 		result = ieee80211_tx_8023(sdata, skb, sta, true);
4805 	} else {
4806 		struct sk_buff_head skbs;
4807 
4808 		__skb_queue_head_init(&skbs);
4809 		__skb_queue_tail(&skbs, skb);
4810 
4811 		hdr = (struct ieee80211_hdr *)skb->data;
4812 		sta = sta_info_get(sdata, hdr->addr1);
4813 
4814 		result = __ieee80211_tx(local, &skbs, sta, true);
4815 	}
4816 
4817 	return result;
4818 }
4819 
4820 /*
4821  * Transmit all pending packets. Called from tasklet.
4822  */
ieee80211_tx_pending(struct tasklet_struct * t)4823 void ieee80211_tx_pending(struct tasklet_struct *t)
4824 {
4825 	struct ieee80211_local *local = from_tasklet(local, t,
4826 						     tx_pending_tasklet);
4827 	unsigned long flags;
4828 	int i;
4829 	bool txok;
4830 
4831 	rcu_read_lock();
4832 
4833 	spin_lock_irqsave(&local->queue_stop_reason_lock, flags);
4834 	for (i = 0; i < local->hw.queues; i++) {
4835 		/*
4836 		 * If queue is stopped by something other than due to pending
4837 		 * frames, or we have no pending frames, proceed to next queue.
4838 		 */
4839 		if (local->queue_stop_reasons[i] ||
4840 		    skb_queue_empty(&local->pending[i]))
4841 			continue;
4842 
4843 		while (!skb_queue_empty(&local->pending[i])) {
4844 			struct sk_buff *skb = __skb_dequeue(&local->pending[i]);
4845 			struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
4846 
4847 			if (WARN_ON(!info->control.vif)) {
4848 				ieee80211_free_txskb(&local->hw, skb);
4849 				continue;
4850 			}
4851 
4852 			spin_unlock_irqrestore(&local->queue_stop_reason_lock,
4853 						flags);
4854 
4855 			txok = ieee80211_tx_pending_skb(local, skb);
4856 			spin_lock_irqsave(&local->queue_stop_reason_lock,
4857 					  flags);
4858 			if (!txok)
4859 				break;
4860 		}
4861 	}
4862 	spin_unlock_irqrestore(&local->queue_stop_reason_lock, flags);
4863 
4864 	rcu_read_unlock();
4865 }
4866 
4867 /* functions for drivers to get certain frames */
4868 
__ieee80211_beacon_add_tim(struct ieee80211_sub_if_data * sdata,struct ieee80211_link_data * link,struct ps_data * ps,struct sk_buff * skb,bool is_template)4869 static void __ieee80211_beacon_add_tim(struct ieee80211_sub_if_data *sdata,
4870 				       struct ieee80211_link_data *link,
4871 				       struct ps_data *ps, struct sk_buff *skb,
4872 				       bool is_template)
4873 {
4874 	u8 *pos, *tim;
4875 	int aid0 = 0;
4876 	int i, have_bits = 0, n1, n2;
4877 	struct ieee80211_bss_conf *link_conf = link->conf;
4878 
4879 	/* Generate bitmap for TIM only if there are any STAs in power save
4880 	 * mode. */
4881 	if (atomic_read(&ps->num_sta_ps) > 0)
4882 		/* in the hope that this is faster than
4883 		 * checking byte-for-byte */
4884 		have_bits = !bitmap_empty((unsigned long *)ps->tim,
4885 					  IEEE80211_MAX_AID+1);
4886 	if (!is_template) {
4887 		if (ps->dtim_count == 0)
4888 			ps->dtim_count = link_conf->dtim_period - 1;
4889 		else
4890 			ps->dtim_count--;
4891 	}
4892 
4893 	tim = pos = skb_put(skb, 5);
4894 	*pos++ = WLAN_EID_TIM;
4895 	*pos++ = 3;
4896 	*pos++ = ps->dtim_count;
4897 	*pos++ = link_conf->dtim_period;
4898 
4899 	if (ps->dtim_count == 0 && !skb_queue_empty(&ps->bc_buf))
4900 		aid0 = 1;
4901 
4902 	ps->dtim_bc_mc = aid0 == 1;
4903 
4904 	if (have_bits) {
4905 		/* Find largest even number N1 so that bits numbered 1 through
4906 		 * (N1 x 8) - 1 in the bitmap are 0 and number N2 so that bits
4907 		 * (N2 + 1) x 8 through 2007 are 0. */
4908 		n1 = 0;
4909 		for (i = 0; i < IEEE80211_MAX_TIM_LEN; i++) {
4910 			if (ps->tim[i]) {
4911 				n1 = i & 0xfe;
4912 				break;
4913 			}
4914 		}
4915 		n2 = n1;
4916 		for (i = IEEE80211_MAX_TIM_LEN - 1; i >= n1; i--) {
4917 			if (ps->tim[i]) {
4918 				n2 = i;
4919 				break;
4920 			}
4921 		}
4922 
4923 		/* Bitmap control */
4924 		*pos++ = n1 | aid0;
4925 		/* Part Virt Bitmap */
4926 		skb_put_data(skb, ps->tim + n1, n2 - n1 + 1);
4927 
4928 		tim[1] = n2 - n1 + 4;
4929 	} else {
4930 		*pos++ = aid0; /* Bitmap control */
4931 
4932 		if (ieee80211_get_link_sband(link)->band != NL80211_BAND_S1GHZ) {
4933 			tim[1] = 4;
4934 			/* Part Virt Bitmap */
4935 			skb_put_u8(skb, 0);
4936 		}
4937 	}
4938 }
4939 
ieee80211_beacon_add_tim(struct ieee80211_sub_if_data * sdata,struct ieee80211_link_data * link,struct ps_data * ps,struct sk_buff * skb,bool is_template)4940 static int ieee80211_beacon_add_tim(struct ieee80211_sub_if_data *sdata,
4941 				    struct ieee80211_link_data *link,
4942 				    struct ps_data *ps, struct sk_buff *skb,
4943 				    bool is_template)
4944 {
4945 	struct ieee80211_local *local = sdata->local;
4946 
4947 	/*
4948 	 * Not very nice, but we want to allow the driver to call
4949 	 * ieee80211_beacon_get() as a response to the set_tim()
4950 	 * callback. That, however, is already invoked under the
4951 	 * sta_lock to guarantee consistent and race-free update
4952 	 * of the tim bitmap in mac80211 and the driver.
4953 	 */
4954 	if (local->tim_in_locked_section) {
4955 		__ieee80211_beacon_add_tim(sdata, link, ps, skb, is_template);
4956 	} else {
4957 		spin_lock_bh(&local->tim_lock);
4958 		__ieee80211_beacon_add_tim(sdata, link, ps, skb, is_template);
4959 		spin_unlock_bh(&local->tim_lock);
4960 	}
4961 
4962 	return 0;
4963 }
4964 
ieee80211_set_beacon_cntdwn(struct ieee80211_sub_if_data * sdata,struct beacon_data * beacon,struct ieee80211_link_data * link)4965 static void ieee80211_set_beacon_cntdwn(struct ieee80211_sub_if_data *sdata,
4966 					struct beacon_data *beacon,
4967 					struct ieee80211_link_data *link)
4968 {
4969 	u8 *beacon_data, count, max_count = 1;
4970 	struct probe_resp *resp;
4971 	size_t beacon_data_len;
4972 	u16 *bcn_offsets;
4973 	int i;
4974 
4975 	switch (sdata->vif.type) {
4976 	case NL80211_IFTYPE_AP:
4977 		beacon_data = beacon->tail;
4978 		beacon_data_len = beacon->tail_len;
4979 		break;
4980 	case NL80211_IFTYPE_ADHOC:
4981 		beacon_data = beacon->head;
4982 		beacon_data_len = beacon->head_len;
4983 		break;
4984 	case NL80211_IFTYPE_MESH_POINT:
4985 		beacon_data = beacon->head;
4986 		beacon_data_len = beacon->head_len;
4987 		break;
4988 	default:
4989 		return;
4990 	}
4991 
4992 	resp = rcu_dereference(link->u.ap.probe_resp);
4993 
4994 	bcn_offsets = beacon->cntdwn_counter_offsets;
4995 	count = beacon->cntdwn_current_counter;
4996 	if (link->conf->csa_active)
4997 		max_count = IEEE80211_MAX_CNTDWN_COUNTERS_NUM;
4998 
4999 	for (i = 0; i < max_count; ++i) {
5000 		if (bcn_offsets[i]) {
5001 			if (WARN_ON_ONCE(bcn_offsets[i] >= beacon_data_len))
5002 				return;
5003 			beacon_data[bcn_offsets[i]] = count;
5004 		}
5005 
5006 		if (sdata->vif.type == NL80211_IFTYPE_AP && resp) {
5007 			u16 *resp_offsets = resp->cntdwn_counter_offsets;
5008 
5009 			resp->data[resp_offsets[i]] = count;
5010 		}
5011 	}
5012 }
5013 
__ieee80211_beacon_update_cntdwn(struct beacon_data * beacon)5014 static u8 __ieee80211_beacon_update_cntdwn(struct beacon_data *beacon)
5015 {
5016 	beacon->cntdwn_current_counter--;
5017 
5018 	/* the counter should never reach 0 */
5019 	WARN_ON_ONCE(!beacon->cntdwn_current_counter);
5020 
5021 	return beacon->cntdwn_current_counter;
5022 }
5023 
ieee80211_beacon_update_cntdwn(struct ieee80211_vif * vif)5024 u8 ieee80211_beacon_update_cntdwn(struct ieee80211_vif *vif)
5025 {
5026 	struct ieee80211_sub_if_data *sdata = vif_to_sdata(vif);
5027 	struct beacon_data *beacon = NULL;
5028 	u8 count = 0;
5029 
5030 	rcu_read_lock();
5031 
5032 	if (sdata->vif.type == NL80211_IFTYPE_AP)
5033 		beacon = rcu_dereference(sdata->deflink.u.ap.beacon);
5034 	else if (sdata->vif.type == NL80211_IFTYPE_ADHOC)
5035 		beacon = rcu_dereference(sdata->u.ibss.presp);
5036 	else if (ieee80211_vif_is_mesh(&sdata->vif))
5037 		beacon = rcu_dereference(sdata->u.mesh.beacon);
5038 
5039 	if (!beacon)
5040 		goto unlock;
5041 
5042 	count = __ieee80211_beacon_update_cntdwn(beacon);
5043 
5044 unlock:
5045 	rcu_read_unlock();
5046 	return count;
5047 }
5048 EXPORT_SYMBOL(ieee80211_beacon_update_cntdwn);
5049 
ieee80211_beacon_set_cntdwn(struct ieee80211_vif * vif,u8 counter)5050 void ieee80211_beacon_set_cntdwn(struct ieee80211_vif *vif, u8 counter)
5051 {
5052 	struct ieee80211_sub_if_data *sdata = vif_to_sdata(vif);
5053 	struct beacon_data *beacon = NULL;
5054 
5055 	rcu_read_lock();
5056 
5057 	if (sdata->vif.type == NL80211_IFTYPE_AP)
5058 		beacon = rcu_dereference(sdata->deflink.u.ap.beacon);
5059 	else if (sdata->vif.type == NL80211_IFTYPE_ADHOC)
5060 		beacon = rcu_dereference(sdata->u.ibss.presp);
5061 	else if (ieee80211_vif_is_mesh(&sdata->vif))
5062 		beacon = rcu_dereference(sdata->u.mesh.beacon);
5063 
5064 	if (!beacon)
5065 		goto unlock;
5066 
5067 	if (counter < beacon->cntdwn_current_counter)
5068 		beacon->cntdwn_current_counter = counter;
5069 
5070 unlock:
5071 	rcu_read_unlock();
5072 }
5073 EXPORT_SYMBOL(ieee80211_beacon_set_cntdwn);
5074 
ieee80211_beacon_cntdwn_is_complete(struct ieee80211_vif * vif)5075 bool ieee80211_beacon_cntdwn_is_complete(struct ieee80211_vif *vif)
5076 {
5077 	struct ieee80211_sub_if_data *sdata = vif_to_sdata(vif);
5078 	struct beacon_data *beacon = NULL;
5079 	u8 *beacon_data;
5080 	size_t beacon_data_len;
5081 	int ret = false;
5082 
5083 	if (!ieee80211_sdata_running(sdata))
5084 		return false;
5085 
5086 	rcu_read_lock();
5087 	if (vif->type == NL80211_IFTYPE_AP) {
5088 		beacon = rcu_dereference(sdata->deflink.u.ap.beacon);
5089 		if (WARN_ON(!beacon || !beacon->tail))
5090 			goto out;
5091 		beacon_data = beacon->tail;
5092 		beacon_data_len = beacon->tail_len;
5093 	} else if (vif->type == NL80211_IFTYPE_ADHOC) {
5094 		struct ieee80211_if_ibss *ifibss = &sdata->u.ibss;
5095 
5096 		beacon = rcu_dereference(ifibss->presp);
5097 		if (!beacon)
5098 			goto out;
5099 
5100 		beacon_data = beacon->head;
5101 		beacon_data_len = beacon->head_len;
5102 	} else if (vif->type == NL80211_IFTYPE_MESH_POINT) {
5103 		struct ieee80211_if_mesh *ifmsh = &sdata->u.mesh;
5104 
5105 		beacon = rcu_dereference(ifmsh->beacon);
5106 		if (!beacon)
5107 			goto out;
5108 
5109 		beacon_data = beacon->head;
5110 		beacon_data_len = beacon->head_len;
5111 	} else {
5112 		WARN_ON(1);
5113 		goto out;
5114 	}
5115 
5116 	if (!beacon->cntdwn_counter_offsets[0])
5117 		goto out;
5118 
5119 	if (WARN_ON_ONCE(beacon->cntdwn_counter_offsets[0] > beacon_data_len))
5120 		goto out;
5121 
5122 	if (beacon_data[beacon->cntdwn_counter_offsets[0]] == 1)
5123 		ret = true;
5124 
5125  out:
5126 	rcu_read_unlock();
5127 
5128 	return ret;
5129 }
5130 EXPORT_SYMBOL(ieee80211_beacon_cntdwn_is_complete);
5131 
ieee80211_beacon_protect(struct sk_buff * skb,struct ieee80211_local * local,struct ieee80211_sub_if_data * sdata,struct ieee80211_link_data * link)5132 static int ieee80211_beacon_protect(struct sk_buff *skb,
5133 				    struct ieee80211_local *local,
5134 				    struct ieee80211_sub_if_data *sdata,
5135 				    struct ieee80211_link_data *link)
5136 {
5137 	ieee80211_tx_result res;
5138 	struct ieee80211_tx_data tx;
5139 	struct sk_buff *check_skb;
5140 
5141 	memset(&tx, 0, sizeof(tx));
5142 	tx.key = rcu_dereference(link->default_beacon_key);
5143 	if (!tx.key)
5144 		return 0;
5145 
5146 	if (unlikely(tx.key->flags & KEY_FLAG_TAINTED)) {
5147 		tx.key = NULL;
5148 		return -EINVAL;
5149 	}
5150 
5151 	if (!(tx.key->conf.flags & IEEE80211_KEY_FLAG_SW_MGMT_TX) &&
5152 	    tx.key->flags & KEY_FLAG_UPLOADED_TO_HARDWARE)
5153 		IEEE80211_SKB_CB(skb)->control.hw_key = &tx.key->conf;
5154 
5155 	tx.local = local;
5156 	tx.sdata = sdata;
5157 	__skb_queue_head_init(&tx.skbs);
5158 	__skb_queue_tail(&tx.skbs, skb);
5159 	res = ieee80211_tx_h_encrypt(&tx);
5160 	check_skb = __skb_dequeue(&tx.skbs);
5161 	/* we may crash after this, but it'd be a bug in crypto */
5162 	WARN_ON(check_skb != skb);
5163 	if (WARN_ON_ONCE(res != TX_CONTINUE))
5164 		return -EINVAL;
5165 
5166 	return 0;
5167 }
5168 
5169 static void
ieee80211_beacon_get_finish(struct ieee80211_hw * hw,struct ieee80211_vif * vif,struct ieee80211_link_data * link,struct ieee80211_mutable_offsets * offs,struct beacon_data * beacon,struct sk_buff * skb,struct ieee80211_chanctx_conf * chanctx_conf,u16 csa_off_base)5170 ieee80211_beacon_get_finish(struct ieee80211_hw *hw,
5171 			    struct ieee80211_vif *vif,
5172 			    struct ieee80211_link_data *link,
5173 			    struct ieee80211_mutable_offsets *offs,
5174 			    struct beacon_data *beacon,
5175 			    struct sk_buff *skb,
5176 			    struct ieee80211_chanctx_conf *chanctx_conf,
5177 			    u16 csa_off_base)
5178 {
5179 	struct ieee80211_local *local = hw_to_local(hw);
5180 	struct ieee80211_sub_if_data *sdata = vif_to_sdata(vif);
5181 	struct ieee80211_tx_info *info;
5182 	enum nl80211_band band;
5183 	struct ieee80211_tx_rate_control txrc;
5184 
5185 	/* CSA offsets */
5186 	if (offs && beacon) {
5187 		u16 i;
5188 
5189 		for (i = 0; i < IEEE80211_MAX_CNTDWN_COUNTERS_NUM; i++) {
5190 			u16 csa_off = beacon->cntdwn_counter_offsets[i];
5191 
5192 			if (!csa_off)
5193 				continue;
5194 
5195 			offs->cntdwn_counter_offs[i] = csa_off_base + csa_off;
5196 		}
5197 	}
5198 
5199 	band = chanctx_conf->def.chan->band;
5200 	info = IEEE80211_SKB_CB(skb);
5201 	info->flags |= IEEE80211_TX_INTFL_DONT_ENCRYPT;
5202 	info->flags |= IEEE80211_TX_CTL_NO_ACK;
5203 	info->band = band;
5204 
5205 	memset(&txrc, 0, sizeof(txrc));
5206 	txrc.hw = hw;
5207 	txrc.sband = local->hw.wiphy->bands[band];
5208 	txrc.bss_conf = link->conf;
5209 	txrc.skb = skb;
5210 	txrc.reported_rate.idx = -1;
5211 	if (sdata->beacon_rate_set && sdata->beacon_rateidx_mask[band])
5212 		txrc.rate_idx_mask = sdata->beacon_rateidx_mask[band];
5213 	else
5214 		txrc.rate_idx_mask = sdata->rc_rateidx_mask[band];
5215 	txrc.bss = true;
5216 	rate_control_get_rate(sdata, NULL, &txrc);
5217 
5218 	info->control.vif = vif;
5219 	info->control.flags |= u32_encode_bits(link->link_id,
5220 					       IEEE80211_TX_CTRL_MLO_LINK);
5221 	info->flags |= IEEE80211_TX_CTL_CLEAR_PS_FILT |
5222 		       IEEE80211_TX_CTL_ASSIGN_SEQ |
5223 		       IEEE80211_TX_CTL_FIRST_FRAGMENT;
5224 }
5225 
5226 static void
ieee80211_beacon_add_mbssid(struct sk_buff * skb,struct beacon_data * beacon,u8 i)5227 ieee80211_beacon_add_mbssid(struct sk_buff *skb, struct beacon_data *beacon,
5228 			    u8 i)
5229 {
5230 	if (!beacon->mbssid_ies || !beacon->mbssid_ies->cnt ||
5231 	    i > beacon->mbssid_ies->cnt)
5232 		return;
5233 
5234 	if (i < beacon->mbssid_ies->cnt) {
5235 		skb_put_data(skb, beacon->mbssid_ies->elem[i].data,
5236 			     beacon->mbssid_ies->elem[i].len);
5237 
5238 		if (beacon->rnr_ies && beacon->rnr_ies->cnt) {
5239 			skb_put_data(skb, beacon->rnr_ies->elem[i].data,
5240 				     beacon->rnr_ies->elem[i].len);
5241 
5242 			for (i = beacon->mbssid_ies->cnt; i < beacon->rnr_ies->cnt; i++)
5243 				skb_put_data(skb, beacon->rnr_ies->elem[i].data,
5244 					     beacon->rnr_ies->elem[i].len);
5245 		}
5246 		return;
5247 	}
5248 
5249 	/* i == beacon->mbssid_ies->cnt, include all MBSSID elements */
5250 	for (i = 0; i < beacon->mbssid_ies->cnt; i++)
5251 		skb_put_data(skb, beacon->mbssid_ies->elem[i].data,
5252 			     beacon->mbssid_ies->elem[i].len);
5253 }
5254 
5255 static struct sk_buff *
ieee80211_beacon_get_ap(struct ieee80211_hw * hw,struct ieee80211_vif * vif,struct ieee80211_link_data * link,struct ieee80211_mutable_offsets * offs,bool is_template,struct beacon_data * beacon,struct ieee80211_chanctx_conf * chanctx_conf,u8 ema_index)5256 ieee80211_beacon_get_ap(struct ieee80211_hw *hw,
5257 			struct ieee80211_vif *vif,
5258 			struct ieee80211_link_data *link,
5259 			struct ieee80211_mutable_offsets *offs,
5260 			bool is_template,
5261 			struct beacon_data *beacon,
5262 			struct ieee80211_chanctx_conf *chanctx_conf,
5263 			u8 ema_index)
5264 {
5265 	struct ieee80211_local *local = hw_to_local(hw);
5266 	struct ieee80211_sub_if_data *sdata = vif_to_sdata(vif);
5267 	struct ieee80211_if_ap *ap = &sdata->u.ap;
5268 	struct sk_buff *skb = NULL;
5269 	u16 csa_off_base = 0;
5270 	int mbssid_len;
5271 
5272 	if (beacon->cntdwn_counter_offsets[0]) {
5273 		if (!is_template)
5274 			ieee80211_beacon_update_cntdwn(vif);
5275 
5276 		ieee80211_set_beacon_cntdwn(sdata, beacon, link);
5277 	}
5278 
5279 	/* headroom, head length,
5280 	 * tail length, maximum TIM length and multiple BSSID length
5281 	 */
5282 	mbssid_len = ieee80211_get_mbssid_beacon_len(beacon->mbssid_ies,
5283 						     beacon->rnr_ies,
5284 						     ema_index);
5285 
5286 	skb = dev_alloc_skb(local->tx_headroom + beacon->head_len +
5287 			    beacon->tail_len + 256 +
5288 			    local->hw.extra_beacon_tailroom + mbssid_len);
5289 	if (!skb)
5290 		return NULL;
5291 
5292 	skb_reserve(skb, local->tx_headroom);
5293 	skb_put_data(skb, beacon->head, beacon->head_len);
5294 
5295 	ieee80211_beacon_add_tim(sdata, link, &ap->ps, skb, is_template);
5296 
5297 	if (offs) {
5298 		offs->tim_offset = beacon->head_len;
5299 		offs->tim_length = skb->len - beacon->head_len;
5300 		offs->cntdwn_counter_offs[0] = beacon->cntdwn_counter_offsets[0];
5301 
5302 		if (mbssid_len) {
5303 			ieee80211_beacon_add_mbssid(skb, beacon, ema_index);
5304 			offs->mbssid_off = skb->len - mbssid_len;
5305 		}
5306 
5307 		/* for AP the csa offsets are from tail */
5308 		csa_off_base = skb->len;
5309 	}
5310 
5311 	if (beacon->tail)
5312 		skb_put_data(skb, beacon->tail, beacon->tail_len);
5313 
5314 	if (ieee80211_beacon_protect(skb, local, sdata, link) < 0) {
5315 		dev_kfree_skb(skb);
5316 		return NULL;
5317 	}
5318 
5319 	ieee80211_beacon_get_finish(hw, vif, link, offs, beacon, skb,
5320 				    chanctx_conf, csa_off_base);
5321 	return skb;
5322 }
5323 
5324 static struct ieee80211_ema_beacons *
ieee80211_beacon_get_ap_ema_list(struct ieee80211_hw * hw,struct ieee80211_vif * vif,struct ieee80211_link_data * link,struct ieee80211_mutable_offsets * offs,bool is_template,struct beacon_data * beacon,struct ieee80211_chanctx_conf * chanctx_conf)5325 ieee80211_beacon_get_ap_ema_list(struct ieee80211_hw *hw,
5326 				 struct ieee80211_vif *vif,
5327 				 struct ieee80211_link_data *link,
5328 				 struct ieee80211_mutable_offsets *offs,
5329 				 bool is_template, struct beacon_data *beacon,
5330 				 struct ieee80211_chanctx_conf *chanctx_conf)
5331 {
5332 	struct ieee80211_ema_beacons *ema = NULL;
5333 
5334 	if (!beacon->mbssid_ies || !beacon->mbssid_ies->cnt)
5335 		return NULL;
5336 
5337 	ema = kzalloc(struct_size(ema, bcn, beacon->mbssid_ies->cnt),
5338 		      GFP_ATOMIC);
5339 	if (!ema)
5340 		return NULL;
5341 
5342 	for (ema->cnt = 0; ema->cnt < beacon->mbssid_ies->cnt; ema->cnt++) {
5343 		ema->bcn[ema->cnt].skb =
5344 			ieee80211_beacon_get_ap(hw, vif, link,
5345 						&ema->bcn[ema->cnt].offs,
5346 						is_template, beacon,
5347 						chanctx_conf, ema->cnt);
5348 		if (!ema->bcn[ema->cnt].skb)
5349 			break;
5350 	}
5351 
5352 	if (ema->cnt == beacon->mbssid_ies->cnt)
5353 		return ema;
5354 
5355 	ieee80211_beacon_free_ema_list(ema);
5356 	return NULL;
5357 }
5358 
5359 #define IEEE80211_INCLUDE_ALL_MBSSID_ELEMS -1
5360 
5361 static struct sk_buff *
__ieee80211_beacon_get(struct ieee80211_hw * hw,struct ieee80211_vif * vif,struct ieee80211_mutable_offsets * offs,bool is_template,unsigned int link_id,int ema_index,struct ieee80211_ema_beacons ** ema_beacons)5362 __ieee80211_beacon_get(struct ieee80211_hw *hw,
5363 		       struct ieee80211_vif *vif,
5364 		       struct ieee80211_mutable_offsets *offs,
5365 		       bool is_template,
5366 		       unsigned int link_id,
5367 		       int ema_index,
5368 		       struct ieee80211_ema_beacons **ema_beacons)
5369 {
5370 	struct ieee80211_local *local = hw_to_local(hw);
5371 	struct beacon_data *beacon = NULL;
5372 	struct sk_buff *skb = NULL;
5373 	struct ieee80211_sub_if_data *sdata = NULL;
5374 	struct ieee80211_chanctx_conf *chanctx_conf;
5375 	struct ieee80211_link_data *link;
5376 
5377 	rcu_read_lock();
5378 
5379 	sdata = vif_to_sdata(vif);
5380 	link = rcu_dereference(sdata->link[link_id]);
5381 	if (!link)
5382 		goto out;
5383 	chanctx_conf =
5384 		rcu_dereference(link->conf->chanctx_conf);
5385 
5386 	if (!ieee80211_sdata_running(sdata) || !chanctx_conf)
5387 		goto out;
5388 
5389 	if (offs)
5390 		memset(offs, 0, sizeof(*offs));
5391 
5392 	if (sdata->vif.type == NL80211_IFTYPE_AP) {
5393 		beacon = rcu_dereference(link->u.ap.beacon);
5394 		if (!beacon)
5395 			goto out;
5396 
5397 		if (ema_beacons) {
5398 			*ema_beacons =
5399 				ieee80211_beacon_get_ap_ema_list(hw, vif, link,
5400 								 offs,
5401 								 is_template,
5402 								 beacon,
5403 								 chanctx_conf);
5404 		} else {
5405 			if (beacon->mbssid_ies && beacon->mbssid_ies->cnt) {
5406 				if (ema_index >= beacon->mbssid_ies->cnt)
5407 					goto out; /* End of MBSSID elements */
5408 
5409 				if (ema_index <= IEEE80211_INCLUDE_ALL_MBSSID_ELEMS)
5410 					ema_index = beacon->mbssid_ies->cnt;
5411 			} else {
5412 				ema_index = 0;
5413 			}
5414 
5415 			skb = ieee80211_beacon_get_ap(hw, vif, link, offs,
5416 						      is_template, beacon,
5417 						      chanctx_conf,
5418 						      ema_index);
5419 		}
5420 	} else if (sdata->vif.type == NL80211_IFTYPE_ADHOC) {
5421 		struct ieee80211_if_ibss *ifibss = &sdata->u.ibss;
5422 		struct ieee80211_hdr *hdr;
5423 
5424 		beacon = rcu_dereference(ifibss->presp);
5425 		if (!beacon)
5426 			goto out;
5427 
5428 		if (beacon->cntdwn_counter_offsets[0]) {
5429 			if (!is_template)
5430 				__ieee80211_beacon_update_cntdwn(beacon);
5431 
5432 			ieee80211_set_beacon_cntdwn(sdata, beacon, link);
5433 		}
5434 
5435 		skb = dev_alloc_skb(local->tx_headroom + beacon->head_len +
5436 				    local->hw.extra_beacon_tailroom);
5437 		if (!skb)
5438 			goto out;
5439 		skb_reserve(skb, local->tx_headroom);
5440 		skb_put_data(skb, beacon->head, beacon->head_len);
5441 
5442 		hdr = (struct ieee80211_hdr *) skb->data;
5443 		hdr->frame_control = cpu_to_le16(IEEE80211_FTYPE_MGMT |
5444 						 IEEE80211_STYPE_BEACON);
5445 
5446 		ieee80211_beacon_get_finish(hw, vif, link, offs, beacon, skb,
5447 					    chanctx_conf, 0);
5448 	} else if (ieee80211_vif_is_mesh(&sdata->vif)) {
5449 		struct ieee80211_if_mesh *ifmsh = &sdata->u.mesh;
5450 
5451 		beacon = rcu_dereference(ifmsh->beacon);
5452 		if (!beacon)
5453 			goto out;
5454 
5455 		if (beacon->cntdwn_counter_offsets[0]) {
5456 			if (!is_template)
5457 				/* TODO: For mesh csa_counter is in TU, so
5458 				 * decrementing it by one isn't correct, but
5459 				 * for now we leave it consistent with overall
5460 				 * mac80211's behavior.
5461 				 */
5462 				__ieee80211_beacon_update_cntdwn(beacon);
5463 
5464 			ieee80211_set_beacon_cntdwn(sdata, beacon, link);
5465 		}
5466 
5467 		if (ifmsh->sync_ops)
5468 			ifmsh->sync_ops->adjust_tsf(sdata, beacon);
5469 
5470 		skb = dev_alloc_skb(local->tx_headroom +
5471 				    beacon->head_len +
5472 				    256 + /* TIM IE */
5473 				    beacon->tail_len +
5474 				    local->hw.extra_beacon_tailroom);
5475 		if (!skb)
5476 			goto out;
5477 		skb_reserve(skb, local->tx_headroom);
5478 		skb_put_data(skb, beacon->head, beacon->head_len);
5479 		ieee80211_beacon_add_tim(sdata, link, &ifmsh->ps, skb,
5480 					 is_template);
5481 
5482 		if (offs) {
5483 			offs->tim_offset = beacon->head_len;
5484 			offs->tim_length = skb->len - beacon->head_len;
5485 		}
5486 
5487 		skb_put_data(skb, beacon->tail, beacon->tail_len);
5488 		ieee80211_beacon_get_finish(hw, vif, link, offs, beacon, skb,
5489 					    chanctx_conf, 0);
5490 	} else {
5491 		WARN_ON(1);
5492 		goto out;
5493 	}
5494 
5495  out:
5496 	rcu_read_unlock();
5497 	return skb;
5498 
5499 }
5500 
5501 struct sk_buff *
ieee80211_beacon_get_template(struct ieee80211_hw * hw,struct ieee80211_vif * vif,struct ieee80211_mutable_offsets * offs,unsigned int link_id)5502 ieee80211_beacon_get_template(struct ieee80211_hw *hw,
5503 			      struct ieee80211_vif *vif,
5504 			      struct ieee80211_mutable_offsets *offs,
5505 			      unsigned int link_id)
5506 {
5507 	return __ieee80211_beacon_get(hw, vif, offs, true, link_id,
5508 				      IEEE80211_INCLUDE_ALL_MBSSID_ELEMS, NULL);
5509 }
5510 EXPORT_SYMBOL(ieee80211_beacon_get_template);
5511 
5512 struct sk_buff *
ieee80211_beacon_get_template_ema_index(struct ieee80211_hw * hw,struct ieee80211_vif * vif,struct ieee80211_mutable_offsets * offs,unsigned int link_id,u8 ema_index)5513 ieee80211_beacon_get_template_ema_index(struct ieee80211_hw *hw,
5514 					struct ieee80211_vif *vif,
5515 					struct ieee80211_mutable_offsets *offs,
5516 					unsigned int link_id, u8 ema_index)
5517 {
5518 	return __ieee80211_beacon_get(hw, vif, offs, true, link_id, ema_index,
5519 				      NULL);
5520 }
5521 EXPORT_SYMBOL(ieee80211_beacon_get_template_ema_index);
5522 
ieee80211_beacon_free_ema_list(struct ieee80211_ema_beacons * ema_beacons)5523 void ieee80211_beacon_free_ema_list(struct ieee80211_ema_beacons *ema_beacons)
5524 {
5525 	u8 i;
5526 
5527 	if (!ema_beacons)
5528 		return;
5529 
5530 	for (i = 0; i < ema_beacons->cnt; i++)
5531 		kfree_skb(ema_beacons->bcn[i].skb);
5532 
5533 	kfree(ema_beacons);
5534 }
5535 EXPORT_SYMBOL(ieee80211_beacon_free_ema_list);
5536 
5537 struct ieee80211_ema_beacons *
ieee80211_beacon_get_template_ema_list(struct ieee80211_hw * hw,struct ieee80211_vif * vif,unsigned int link_id)5538 ieee80211_beacon_get_template_ema_list(struct ieee80211_hw *hw,
5539 				       struct ieee80211_vif *vif,
5540 				       unsigned int link_id)
5541 {
5542 	struct ieee80211_ema_beacons *ema_beacons = NULL;
5543 
5544 	WARN_ON(__ieee80211_beacon_get(hw, vif, NULL, true, link_id, 0,
5545 				       &ema_beacons));
5546 
5547 	return ema_beacons;
5548 }
5549 EXPORT_SYMBOL(ieee80211_beacon_get_template_ema_list);
5550 
ieee80211_beacon_get_tim(struct ieee80211_hw * hw,struct ieee80211_vif * vif,u16 * tim_offset,u16 * tim_length,unsigned int link_id)5551 struct sk_buff *ieee80211_beacon_get_tim(struct ieee80211_hw *hw,
5552 					 struct ieee80211_vif *vif,
5553 					 u16 *tim_offset, u16 *tim_length,
5554 					 unsigned int link_id)
5555 {
5556 	struct ieee80211_mutable_offsets offs = {};
5557 	struct sk_buff *bcn = __ieee80211_beacon_get(hw, vif, &offs, false,
5558 						     link_id,
5559 						     IEEE80211_INCLUDE_ALL_MBSSID_ELEMS,
5560 						     NULL);
5561 	struct sk_buff *copy;
5562 	int shift;
5563 
5564 	if (!bcn)
5565 		return bcn;
5566 
5567 	if (tim_offset)
5568 		*tim_offset = offs.tim_offset;
5569 
5570 	if (tim_length)
5571 		*tim_length = offs.tim_length;
5572 
5573 	if (ieee80211_hw_check(hw, BEACON_TX_STATUS) ||
5574 	    !hw_to_local(hw)->monitors)
5575 		return bcn;
5576 
5577 	/* send a copy to monitor interfaces */
5578 	copy = skb_copy(bcn, GFP_ATOMIC);
5579 	if (!copy)
5580 		return bcn;
5581 
5582 	shift = ieee80211_vif_get_shift(vif);
5583 	ieee80211_tx_monitor(hw_to_local(hw), copy, 1, shift, false, NULL);
5584 
5585 	return bcn;
5586 }
5587 EXPORT_SYMBOL(ieee80211_beacon_get_tim);
5588 
ieee80211_proberesp_get(struct ieee80211_hw * hw,struct ieee80211_vif * vif)5589 struct sk_buff *ieee80211_proberesp_get(struct ieee80211_hw *hw,
5590 					struct ieee80211_vif *vif)
5591 {
5592 	struct sk_buff *skb = NULL;
5593 	struct probe_resp *presp = NULL;
5594 	struct ieee80211_hdr *hdr;
5595 	struct ieee80211_sub_if_data *sdata = vif_to_sdata(vif);
5596 
5597 	if (sdata->vif.type != NL80211_IFTYPE_AP)
5598 		return NULL;
5599 
5600 	rcu_read_lock();
5601 	presp = rcu_dereference(sdata->deflink.u.ap.probe_resp);
5602 	if (!presp)
5603 		goto out;
5604 
5605 	skb = dev_alloc_skb(presp->len);
5606 	if (!skb)
5607 		goto out;
5608 
5609 	skb_put_data(skb, presp->data, presp->len);
5610 
5611 	hdr = (struct ieee80211_hdr *) skb->data;
5612 	memset(hdr->addr1, 0, sizeof(hdr->addr1));
5613 
5614 out:
5615 	rcu_read_unlock();
5616 	return skb;
5617 }
5618 EXPORT_SYMBOL(ieee80211_proberesp_get);
5619 
ieee80211_get_fils_discovery_tmpl(struct ieee80211_hw * hw,struct ieee80211_vif * vif)5620 struct sk_buff *ieee80211_get_fils_discovery_tmpl(struct ieee80211_hw *hw,
5621 						  struct ieee80211_vif *vif)
5622 {
5623 	struct sk_buff *skb = NULL;
5624 	struct fils_discovery_data *tmpl = NULL;
5625 	struct ieee80211_sub_if_data *sdata = vif_to_sdata(vif);
5626 
5627 	if (sdata->vif.type != NL80211_IFTYPE_AP)
5628 		return NULL;
5629 
5630 	rcu_read_lock();
5631 	tmpl = rcu_dereference(sdata->deflink.u.ap.fils_discovery);
5632 	if (!tmpl) {
5633 		rcu_read_unlock();
5634 		return NULL;
5635 	}
5636 
5637 	skb = dev_alloc_skb(sdata->local->hw.extra_tx_headroom + tmpl->len);
5638 	if (skb) {
5639 		skb_reserve(skb, sdata->local->hw.extra_tx_headroom);
5640 		skb_put_data(skb, tmpl->data, tmpl->len);
5641 	}
5642 
5643 	rcu_read_unlock();
5644 	return skb;
5645 }
5646 EXPORT_SYMBOL(ieee80211_get_fils_discovery_tmpl);
5647 
5648 struct sk_buff *
ieee80211_get_unsol_bcast_probe_resp_tmpl(struct ieee80211_hw * hw,struct ieee80211_vif * vif)5649 ieee80211_get_unsol_bcast_probe_resp_tmpl(struct ieee80211_hw *hw,
5650 					  struct ieee80211_vif *vif)
5651 {
5652 	struct sk_buff *skb = NULL;
5653 	struct unsol_bcast_probe_resp_data *tmpl = NULL;
5654 	struct ieee80211_sub_if_data *sdata = vif_to_sdata(vif);
5655 
5656 	if (sdata->vif.type != NL80211_IFTYPE_AP)
5657 		return NULL;
5658 
5659 	rcu_read_lock();
5660 	tmpl = rcu_dereference(sdata->deflink.u.ap.unsol_bcast_probe_resp);
5661 	if (!tmpl) {
5662 		rcu_read_unlock();
5663 		return NULL;
5664 	}
5665 
5666 	skb = dev_alloc_skb(sdata->local->hw.extra_tx_headroom + tmpl->len);
5667 	if (skb) {
5668 		skb_reserve(skb, sdata->local->hw.extra_tx_headroom);
5669 		skb_put_data(skb, tmpl->data, tmpl->len);
5670 	}
5671 
5672 	rcu_read_unlock();
5673 	return skb;
5674 }
5675 EXPORT_SYMBOL(ieee80211_get_unsol_bcast_probe_resp_tmpl);
5676 
ieee80211_pspoll_get(struct ieee80211_hw * hw,struct ieee80211_vif * vif)5677 struct sk_buff *ieee80211_pspoll_get(struct ieee80211_hw *hw,
5678 				     struct ieee80211_vif *vif)
5679 {
5680 	struct ieee80211_sub_if_data *sdata;
5681 	struct ieee80211_pspoll *pspoll;
5682 	struct ieee80211_local *local;
5683 	struct sk_buff *skb;
5684 
5685 	if (WARN_ON(vif->type != NL80211_IFTYPE_STATION))
5686 		return NULL;
5687 
5688 	sdata = vif_to_sdata(vif);
5689 	local = sdata->local;
5690 
5691 	skb = dev_alloc_skb(local->hw.extra_tx_headroom + sizeof(*pspoll));
5692 	if (!skb)
5693 		return NULL;
5694 
5695 	skb_reserve(skb, local->hw.extra_tx_headroom);
5696 
5697 	pspoll = skb_put_zero(skb, sizeof(*pspoll));
5698 	pspoll->frame_control = cpu_to_le16(IEEE80211_FTYPE_CTL |
5699 					    IEEE80211_STYPE_PSPOLL);
5700 	pspoll->aid = cpu_to_le16(sdata->vif.cfg.aid);
5701 
5702 	/* aid in PS-Poll has its two MSBs each set to 1 */
5703 	pspoll->aid |= cpu_to_le16(1 << 15 | 1 << 14);
5704 
5705 	memcpy(pspoll->bssid, sdata->deflink.u.mgd.bssid, ETH_ALEN);
5706 	memcpy(pspoll->ta, vif->addr, ETH_ALEN);
5707 
5708 	return skb;
5709 }
5710 EXPORT_SYMBOL(ieee80211_pspoll_get);
5711 
ieee80211_nullfunc_get(struct ieee80211_hw * hw,struct ieee80211_vif * vif,int link_id,bool qos_ok)5712 struct sk_buff *ieee80211_nullfunc_get(struct ieee80211_hw *hw,
5713 				       struct ieee80211_vif *vif,
5714 				       int link_id, bool qos_ok)
5715 {
5716 	struct ieee80211_sub_if_data *sdata = vif_to_sdata(vif);
5717 	struct ieee80211_local *local = sdata->local;
5718 	struct ieee80211_link_data *link = NULL;
5719 	struct ieee80211_hdr_3addr *nullfunc;
5720 	struct sk_buff *skb;
5721 	bool qos = false;
5722 
5723 	if (WARN_ON(vif->type != NL80211_IFTYPE_STATION))
5724 		return NULL;
5725 
5726 	skb = dev_alloc_skb(local->hw.extra_tx_headroom +
5727 			    sizeof(*nullfunc) + 2);
5728 	if (!skb)
5729 		return NULL;
5730 
5731 	rcu_read_lock();
5732 	if (qos_ok) {
5733 		struct sta_info *sta;
5734 
5735 		sta = sta_info_get(sdata, vif->cfg.ap_addr);
5736 		qos = sta && sta->sta.wme;
5737 	}
5738 
5739 	if (link_id >= 0) {
5740 		link = rcu_dereference(sdata->link[link_id]);
5741 		if (WARN_ON_ONCE(!link)) {
5742 			rcu_read_unlock();
5743 			kfree_skb(skb);
5744 			return NULL;
5745 		}
5746 	}
5747 
5748 	skb_reserve(skb, local->hw.extra_tx_headroom);
5749 
5750 	nullfunc = skb_put_zero(skb, sizeof(*nullfunc));
5751 	nullfunc->frame_control = cpu_to_le16(IEEE80211_FTYPE_DATA |
5752 					      IEEE80211_STYPE_NULLFUNC |
5753 					      IEEE80211_FCTL_TODS);
5754 	if (qos) {
5755 		__le16 qoshdr = cpu_to_le16(7);
5756 
5757 		BUILD_BUG_ON((IEEE80211_STYPE_QOS_NULLFUNC |
5758 			      IEEE80211_STYPE_NULLFUNC) !=
5759 			     IEEE80211_STYPE_QOS_NULLFUNC);
5760 		nullfunc->frame_control |=
5761 			cpu_to_le16(IEEE80211_STYPE_QOS_NULLFUNC);
5762 		skb->priority = 7;
5763 		skb_set_queue_mapping(skb, IEEE80211_AC_VO);
5764 		skb_put_data(skb, &qoshdr, sizeof(qoshdr));
5765 	}
5766 
5767 	if (link) {
5768 		memcpy(nullfunc->addr1, link->conf->bssid, ETH_ALEN);
5769 		memcpy(nullfunc->addr2, link->conf->addr, ETH_ALEN);
5770 		memcpy(nullfunc->addr3, link->conf->bssid, ETH_ALEN);
5771 	} else {
5772 		memcpy(nullfunc->addr1, vif->cfg.ap_addr, ETH_ALEN);
5773 		memcpy(nullfunc->addr2, vif->addr, ETH_ALEN);
5774 		memcpy(nullfunc->addr3, vif->cfg.ap_addr, ETH_ALEN);
5775 	}
5776 	rcu_read_unlock();
5777 
5778 	return skb;
5779 }
5780 EXPORT_SYMBOL(ieee80211_nullfunc_get);
5781 
ieee80211_probereq_get(struct ieee80211_hw * hw,const u8 * src_addr,const u8 * ssid,size_t ssid_len,size_t tailroom)5782 struct sk_buff *ieee80211_probereq_get(struct ieee80211_hw *hw,
5783 				       const u8 *src_addr,
5784 				       const u8 *ssid, size_t ssid_len,
5785 				       size_t tailroom)
5786 {
5787 	struct ieee80211_local *local = hw_to_local(hw);
5788 	struct ieee80211_hdr_3addr *hdr;
5789 	struct sk_buff *skb;
5790 	size_t ie_ssid_len;
5791 	u8 *pos;
5792 
5793 	ie_ssid_len = 2 + ssid_len;
5794 
5795 	skb = dev_alloc_skb(local->hw.extra_tx_headroom + sizeof(*hdr) +
5796 			    ie_ssid_len + tailroom);
5797 	if (!skb)
5798 		return NULL;
5799 
5800 	skb_reserve(skb, local->hw.extra_tx_headroom);
5801 
5802 	hdr = skb_put_zero(skb, sizeof(*hdr));
5803 	hdr->frame_control = cpu_to_le16(IEEE80211_FTYPE_MGMT |
5804 					 IEEE80211_STYPE_PROBE_REQ);
5805 	eth_broadcast_addr(hdr->addr1);
5806 	memcpy(hdr->addr2, src_addr, ETH_ALEN);
5807 	eth_broadcast_addr(hdr->addr3);
5808 
5809 	pos = skb_put(skb, ie_ssid_len);
5810 	*pos++ = WLAN_EID_SSID;
5811 	*pos++ = ssid_len;
5812 	if (ssid_len)
5813 		memcpy(pos, ssid, ssid_len);
5814 	pos += ssid_len;
5815 
5816 	return skb;
5817 }
5818 EXPORT_SYMBOL(ieee80211_probereq_get);
5819 
ieee80211_rts_get(struct ieee80211_hw * hw,struct ieee80211_vif * vif,const void * frame,size_t frame_len,const struct ieee80211_tx_info * frame_txctl,struct ieee80211_rts * rts)5820 void ieee80211_rts_get(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
5821 		       const void *frame, size_t frame_len,
5822 		       const struct ieee80211_tx_info *frame_txctl,
5823 		       struct ieee80211_rts *rts)
5824 {
5825 	const struct ieee80211_hdr *hdr = frame;
5826 
5827 	rts->frame_control =
5828 	    cpu_to_le16(IEEE80211_FTYPE_CTL | IEEE80211_STYPE_RTS);
5829 	rts->duration = ieee80211_rts_duration(hw, vif, frame_len,
5830 					       frame_txctl);
5831 	memcpy(rts->ra, hdr->addr1, sizeof(rts->ra));
5832 	memcpy(rts->ta, hdr->addr2, sizeof(rts->ta));
5833 }
5834 EXPORT_SYMBOL(ieee80211_rts_get);
5835 
ieee80211_ctstoself_get(struct ieee80211_hw * hw,struct ieee80211_vif * vif,const void * frame,size_t frame_len,const struct ieee80211_tx_info * frame_txctl,struct ieee80211_cts * cts)5836 void ieee80211_ctstoself_get(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
5837 			     const void *frame, size_t frame_len,
5838 			     const struct ieee80211_tx_info *frame_txctl,
5839 			     struct ieee80211_cts *cts)
5840 {
5841 	const struct ieee80211_hdr *hdr = frame;
5842 
5843 	cts->frame_control =
5844 	    cpu_to_le16(IEEE80211_FTYPE_CTL | IEEE80211_STYPE_CTS);
5845 	cts->duration = ieee80211_ctstoself_duration(hw, vif,
5846 						     frame_len, frame_txctl);
5847 	memcpy(cts->ra, hdr->addr1, sizeof(cts->ra));
5848 }
5849 EXPORT_SYMBOL(ieee80211_ctstoself_get);
5850 
5851 struct sk_buff *
ieee80211_get_buffered_bc(struct ieee80211_hw * hw,struct ieee80211_vif * vif)5852 ieee80211_get_buffered_bc(struct ieee80211_hw *hw,
5853 			  struct ieee80211_vif *vif)
5854 {
5855 	struct ieee80211_local *local = hw_to_local(hw);
5856 	struct sk_buff *skb = NULL;
5857 	struct ieee80211_tx_data tx;
5858 	struct ieee80211_sub_if_data *sdata;
5859 	struct ps_data *ps;
5860 	struct ieee80211_tx_info *info;
5861 	struct ieee80211_chanctx_conf *chanctx_conf;
5862 
5863 	sdata = vif_to_sdata(vif);
5864 
5865 	rcu_read_lock();
5866 	chanctx_conf = rcu_dereference(sdata->vif.bss_conf.chanctx_conf);
5867 
5868 	if (!chanctx_conf)
5869 		goto out;
5870 
5871 	if (sdata->vif.type == NL80211_IFTYPE_AP) {
5872 		struct beacon_data *beacon =
5873 				rcu_dereference(sdata->deflink.u.ap.beacon);
5874 
5875 		if (!beacon || !beacon->head)
5876 			goto out;
5877 
5878 		ps = &sdata->u.ap.ps;
5879 	} else if (ieee80211_vif_is_mesh(&sdata->vif)) {
5880 		ps = &sdata->u.mesh.ps;
5881 	} else {
5882 		goto out;
5883 	}
5884 
5885 	if (ps->dtim_count != 0 || !ps->dtim_bc_mc)
5886 		goto out; /* send buffered bc/mc only after DTIM beacon */
5887 
5888 	while (1) {
5889 		skb = skb_dequeue(&ps->bc_buf);
5890 		if (!skb)
5891 			goto out;
5892 		local->total_ps_buffered--;
5893 
5894 		if (!skb_queue_empty(&ps->bc_buf) && skb->len >= 2) {
5895 			struct ieee80211_hdr *hdr =
5896 				(struct ieee80211_hdr *) skb->data;
5897 			/* more buffered multicast/broadcast frames ==> set
5898 			 * MoreData flag in IEEE 802.11 header to inform PS
5899 			 * STAs */
5900 			hdr->frame_control |=
5901 				cpu_to_le16(IEEE80211_FCTL_MOREDATA);
5902 		}
5903 
5904 		if (sdata->vif.type == NL80211_IFTYPE_AP)
5905 			sdata = IEEE80211_DEV_TO_SUB_IF(skb->dev);
5906 		if (!ieee80211_tx_prepare(sdata, &tx, NULL, skb))
5907 			break;
5908 		ieee80211_free_txskb(hw, skb);
5909 	}
5910 
5911 	info = IEEE80211_SKB_CB(skb);
5912 
5913 	tx.flags |= IEEE80211_TX_PS_BUFFERED;
5914 	info->band = chanctx_conf->def.chan->band;
5915 
5916 	if (invoke_tx_handlers(&tx))
5917 		skb = NULL;
5918  out:
5919 	rcu_read_unlock();
5920 
5921 	return skb;
5922 }
5923 EXPORT_SYMBOL(ieee80211_get_buffered_bc);
5924 
ieee80211_reserve_tid(struct ieee80211_sta * pubsta,u8 tid)5925 int ieee80211_reserve_tid(struct ieee80211_sta *pubsta, u8 tid)
5926 {
5927 	struct sta_info *sta = container_of(pubsta, struct sta_info, sta);
5928 	struct ieee80211_sub_if_data *sdata = sta->sdata;
5929 	struct ieee80211_local *local = sdata->local;
5930 	int ret;
5931 	u32 queues;
5932 
5933 	lockdep_assert_held(&local->sta_mtx);
5934 
5935 	/* only some cases are supported right now */
5936 	switch (sdata->vif.type) {
5937 	case NL80211_IFTYPE_STATION:
5938 	case NL80211_IFTYPE_AP:
5939 	case NL80211_IFTYPE_AP_VLAN:
5940 		break;
5941 	default:
5942 		WARN_ON(1);
5943 		return -EINVAL;
5944 	}
5945 
5946 	if (WARN_ON(tid >= IEEE80211_NUM_UPS))
5947 		return -EINVAL;
5948 
5949 	if (sta->reserved_tid == tid) {
5950 		ret = 0;
5951 		goto out;
5952 	}
5953 
5954 	if (sta->reserved_tid != IEEE80211_TID_UNRESERVED) {
5955 		sdata_err(sdata, "TID reservation already active\n");
5956 		ret = -EALREADY;
5957 		goto out;
5958 	}
5959 
5960 	ieee80211_stop_vif_queues(sdata->local, sdata,
5961 				  IEEE80211_QUEUE_STOP_REASON_RESERVE_TID);
5962 
5963 	synchronize_net();
5964 
5965 	/* Tear down BA sessions so we stop aggregating on this TID */
5966 	if (ieee80211_hw_check(&local->hw, AMPDU_AGGREGATION)) {
5967 		set_sta_flag(sta, WLAN_STA_BLOCK_BA);
5968 		__ieee80211_stop_tx_ba_session(sta, tid,
5969 					       AGG_STOP_LOCAL_REQUEST);
5970 	}
5971 
5972 	queues = BIT(sdata->vif.hw_queue[ieee802_1d_to_ac[tid]]);
5973 	__ieee80211_flush_queues(local, sdata, queues, false);
5974 
5975 	sta->reserved_tid = tid;
5976 
5977 	ieee80211_wake_vif_queues(local, sdata,
5978 				  IEEE80211_QUEUE_STOP_REASON_RESERVE_TID);
5979 
5980 	if (ieee80211_hw_check(&local->hw, AMPDU_AGGREGATION))
5981 		clear_sta_flag(sta, WLAN_STA_BLOCK_BA);
5982 
5983 	ret = 0;
5984  out:
5985 	return ret;
5986 }
5987 EXPORT_SYMBOL(ieee80211_reserve_tid);
5988 
ieee80211_unreserve_tid(struct ieee80211_sta * pubsta,u8 tid)5989 void ieee80211_unreserve_tid(struct ieee80211_sta *pubsta, u8 tid)
5990 {
5991 	struct sta_info *sta = container_of(pubsta, struct sta_info, sta);
5992 	struct ieee80211_sub_if_data *sdata = sta->sdata;
5993 
5994 	lockdep_assert_held(&sdata->local->sta_mtx);
5995 
5996 	/* only some cases are supported right now */
5997 	switch (sdata->vif.type) {
5998 	case NL80211_IFTYPE_STATION:
5999 	case NL80211_IFTYPE_AP:
6000 	case NL80211_IFTYPE_AP_VLAN:
6001 		break;
6002 	default:
6003 		WARN_ON(1);
6004 		return;
6005 	}
6006 
6007 	if (tid != sta->reserved_tid) {
6008 		sdata_err(sdata, "TID to unreserve (%d) isn't reserved\n", tid);
6009 		return;
6010 	}
6011 
6012 	sta->reserved_tid = IEEE80211_TID_UNRESERVED;
6013 }
6014 EXPORT_SYMBOL(ieee80211_unreserve_tid);
6015 
__ieee80211_tx_skb_tid_band(struct ieee80211_sub_if_data * sdata,struct sk_buff * skb,int tid,int link_id,enum nl80211_band band)6016 void __ieee80211_tx_skb_tid_band(struct ieee80211_sub_if_data *sdata,
6017 				 struct sk_buff *skb, int tid, int link_id,
6018 				 enum nl80211_band band)
6019 {
6020 	const struct ieee80211_hdr *hdr = (void *)skb->data;
6021 	int ac = ieee80211_ac_from_tid(tid);
6022 	unsigned int link;
6023 
6024 	skb_reset_mac_header(skb);
6025 	skb_set_queue_mapping(skb, ac);
6026 	skb->priority = tid;
6027 
6028 	skb->dev = sdata->dev;
6029 
6030 	BUILD_BUG_ON(IEEE80211_LINK_UNSPECIFIED < IEEE80211_MLD_MAX_NUM_LINKS);
6031 	BUILD_BUG_ON(!FIELD_FIT(IEEE80211_TX_CTRL_MLO_LINK,
6032 				IEEE80211_LINK_UNSPECIFIED));
6033 
6034 	if (!ieee80211_vif_is_mld(&sdata->vif)) {
6035 		link = 0;
6036 	} else if (link_id >= 0) {
6037 		link = link_id;
6038 	} else if (memcmp(sdata->vif.addr, hdr->addr2, ETH_ALEN) == 0) {
6039 		/* address from the MLD */
6040 		link = IEEE80211_LINK_UNSPECIFIED;
6041 	} else {
6042 		/* otherwise must be addressed from a link */
6043 		rcu_read_lock();
6044 		for (link = 0; link < ARRAY_SIZE(sdata->vif.link_conf); link++) {
6045 			struct ieee80211_bss_conf *link_conf;
6046 
6047 			link_conf = rcu_dereference(sdata->vif.link_conf[link]);
6048 			if (!link_conf)
6049 				continue;
6050 			if (memcmp(link_conf->addr, hdr->addr2, ETH_ALEN) == 0)
6051 				break;
6052 		}
6053 		rcu_read_unlock();
6054 
6055 		if (WARN_ON_ONCE(link == ARRAY_SIZE(sdata->vif.link_conf)))
6056 			link = ffs(sdata->vif.active_links) - 1;
6057 	}
6058 
6059 	IEEE80211_SKB_CB(skb)->control.flags |=
6060 		u32_encode_bits(link, IEEE80211_TX_CTRL_MLO_LINK);
6061 
6062 	/*
6063 	 * The other path calling ieee80211_xmit is from the tasklet,
6064 	 * and while we can handle concurrent transmissions locking
6065 	 * requirements are that we do not come into tx with bhs on.
6066 	 */
6067 	local_bh_disable();
6068 	IEEE80211_SKB_CB(skb)->band = band;
6069 	ieee80211_xmit(sdata, NULL, skb);
6070 	local_bh_enable();
6071 }
6072 
ieee80211_tx_skb_tid(struct ieee80211_sub_if_data * sdata,struct sk_buff * skb,int tid,int link_id)6073 void ieee80211_tx_skb_tid(struct ieee80211_sub_if_data *sdata,
6074 			  struct sk_buff *skb, int tid, int link_id)
6075 {
6076 	struct ieee80211_chanctx_conf *chanctx_conf;
6077 	enum nl80211_band band;
6078 
6079 	rcu_read_lock();
6080 	if (!ieee80211_vif_is_mld(&sdata->vif)) {
6081 		WARN_ON(link_id >= 0);
6082 		chanctx_conf =
6083 			rcu_dereference(sdata->vif.bss_conf.chanctx_conf);
6084 		if (WARN_ON(!chanctx_conf)) {
6085 			rcu_read_unlock();
6086 			kfree_skb(skb);
6087 			return;
6088 		}
6089 		band = chanctx_conf->def.chan->band;
6090 	} else {
6091 		WARN_ON(link_id >= 0 &&
6092 			!(sdata->vif.active_links & BIT(link_id)));
6093 		/* MLD transmissions must not rely on the band */
6094 		band = 0;
6095 	}
6096 
6097 	__ieee80211_tx_skb_tid_band(sdata, skb, tid, link_id, band);
6098 	rcu_read_unlock();
6099 }
6100 
ieee80211_tx_control_port(struct wiphy * wiphy,struct net_device * dev,const u8 * buf,size_t len,const u8 * dest,__be16 proto,bool unencrypted,int link_id,u64 * cookie)6101 int ieee80211_tx_control_port(struct wiphy *wiphy, struct net_device *dev,
6102 			      const u8 *buf, size_t len,
6103 			      const u8 *dest, __be16 proto, bool unencrypted,
6104 			      int link_id, u64 *cookie)
6105 {
6106 	struct ieee80211_sub_if_data *sdata = IEEE80211_DEV_TO_SUB_IF(dev);
6107 	struct ieee80211_local *local = sdata->local;
6108 	struct sta_info *sta;
6109 	struct sk_buff *skb;
6110 	struct ethhdr *ehdr;
6111 	u32 ctrl_flags = 0;
6112 	u32 flags = 0;
6113 	int err;
6114 
6115 	/* Only accept CONTROL_PORT_PROTOCOL configured in CONNECT/ASSOCIATE
6116 	 * or Pre-Authentication
6117 	 */
6118 	if (proto != sdata->control_port_protocol &&
6119 	    proto != cpu_to_be16(ETH_P_PREAUTH))
6120 		return -EINVAL;
6121 
6122 	if (proto == sdata->control_port_protocol)
6123 		ctrl_flags |= IEEE80211_TX_CTRL_PORT_CTRL_PROTO |
6124 			      IEEE80211_TX_CTRL_SKIP_MPATH_LOOKUP;
6125 
6126 	if (unencrypted)
6127 		flags |= IEEE80211_TX_INTFL_DONT_ENCRYPT;
6128 
6129 	if (cookie)
6130 		ctrl_flags |= IEEE80211_TX_CTL_REQ_TX_STATUS;
6131 
6132 	flags |= IEEE80211_TX_INTFL_NL80211_FRAME_TX;
6133 
6134 	skb = dev_alloc_skb(local->hw.extra_tx_headroom +
6135 			    sizeof(struct ethhdr) + len);
6136 	if (!skb)
6137 		return -ENOMEM;
6138 
6139 	skb_reserve(skb, local->hw.extra_tx_headroom + sizeof(struct ethhdr));
6140 
6141 	skb_put_data(skb, buf, len);
6142 
6143 	ehdr = skb_push(skb, sizeof(struct ethhdr));
6144 	memcpy(ehdr->h_dest, dest, ETH_ALEN);
6145 
6146 	/* we may override the SA for MLO STA later */
6147 	if (link_id < 0) {
6148 		ctrl_flags |= u32_encode_bits(IEEE80211_LINK_UNSPECIFIED,
6149 					      IEEE80211_TX_CTRL_MLO_LINK);
6150 		memcpy(ehdr->h_source, sdata->vif.addr, ETH_ALEN);
6151 	} else {
6152 		struct ieee80211_bss_conf *link_conf;
6153 
6154 		ctrl_flags |= u32_encode_bits(link_id,
6155 					      IEEE80211_TX_CTRL_MLO_LINK);
6156 
6157 		rcu_read_lock();
6158 		link_conf = rcu_dereference(sdata->vif.link_conf[link_id]);
6159 		if (!link_conf) {
6160 			dev_kfree_skb(skb);
6161 			rcu_read_unlock();
6162 			return -ENOLINK;
6163 		}
6164 		memcpy(ehdr->h_source, link_conf->addr, ETH_ALEN);
6165 		rcu_read_unlock();
6166 	}
6167 
6168 	ehdr->h_proto = proto;
6169 
6170 	skb->dev = dev;
6171 	skb->protocol = proto;
6172 	skb_reset_network_header(skb);
6173 	skb_reset_mac_header(skb);
6174 
6175 	if (local->hw.queues < IEEE80211_NUM_ACS)
6176 		goto start_xmit;
6177 
6178 	/* update QoS header to prioritize control port frames if possible,
6179 	 * priorization also happens for control port frames send over
6180 	 * AF_PACKET
6181 	 */
6182 	rcu_read_lock();
6183 	err = ieee80211_lookup_ra_sta(sdata, skb, &sta);
6184 	if (err) {
6185 		dev_kfree_skb(skb);
6186 		rcu_read_unlock();
6187 		return err;
6188 	}
6189 
6190 	if (!IS_ERR(sta)) {
6191 		u16 queue = ieee80211_select_queue(sdata, sta, skb);
6192 
6193 		skb_set_queue_mapping(skb, queue);
6194 
6195 		/*
6196 		 * for MLO STA, the SA should be the AP MLD address, but
6197 		 * the link ID has been selected already
6198 		 */
6199 		if (sta && sta->sta.mlo)
6200 			memcpy(ehdr->h_source, sdata->vif.addr, ETH_ALEN);
6201 	}
6202 	rcu_read_unlock();
6203 
6204 start_xmit:
6205 	/* mutex lock is only needed for incrementing the cookie counter */
6206 	mutex_lock(&local->mtx);
6207 
6208 	local_bh_disable();
6209 	__ieee80211_subif_start_xmit(skb, skb->dev, flags, ctrl_flags, cookie);
6210 	local_bh_enable();
6211 
6212 	mutex_unlock(&local->mtx);
6213 
6214 	return 0;
6215 }
6216 
ieee80211_probe_mesh_link(struct wiphy * wiphy,struct net_device * dev,const u8 * buf,size_t len)6217 int ieee80211_probe_mesh_link(struct wiphy *wiphy, struct net_device *dev,
6218 			      const u8 *buf, size_t len)
6219 {
6220 	struct ieee80211_sub_if_data *sdata = IEEE80211_DEV_TO_SUB_IF(dev);
6221 	struct ieee80211_local *local = sdata->local;
6222 	struct sk_buff *skb;
6223 
6224 	skb = dev_alloc_skb(local->hw.extra_tx_headroom + len +
6225 			    30 + /* header size */
6226 			    18); /* 11s header size */
6227 	if (!skb)
6228 		return -ENOMEM;
6229 
6230 	skb_reserve(skb, local->hw.extra_tx_headroom);
6231 	skb_put_data(skb, buf, len);
6232 
6233 	skb->dev = dev;
6234 	skb->protocol = htons(ETH_P_802_3);
6235 	skb_reset_network_header(skb);
6236 	skb_reset_mac_header(skb);
6237 
6238 	local_bh_disable();
6239 	__ieee80211_subif_start_xmit(skb, skb->dev, 0,
6240 				     IEEE80211_TX_CTRL_SKIP_MPATH_LOOKUP,
6241 				     NULL);
6242 	local_bh_enable();
6243 
6244 	return 0;
6245 }
6246