xref: /openbmc/linux/net/mac80211/tx.c (revision de2bdb3d)
1 /*
2  * Copyright 2002-2005, Instant802 Networks, Inc.
3  * Copyright 2005-2006, Devicescape Software, Inc.
4  * Copyright 2006-2007	Jiri Benc <jbenc@suse.cz>
5  * Copyright 2007	Johannes Berg <johannes@sipsolutions.net>
6  * Copyright 2013-2014  Intel Mobile Communications GmbH
7  *
8  * This program is free software; you can redistribute it and/or modify
9  * it under the terms of the GNU General Public License version 2 as
10  * published by the Free Software Foundation.
11  *
12  *
13  * Transmit and frame generation functions.
14  */
15 
16 #include <linux/kernel.h>
17 #include <linux/slab.h>
18 #include <linux/skbuff.h>
19 #include <linux/etherdevice.h>
20 #include <linux/bitmap.h>
21 #include <linux/rcupdate.h>
22 #include <linux/export.h>
23 #include <net/net_namespace.h>
24 #include <net/ieee80211_radiotap.h>
25 #include <net/cfg80211.h>
26 #include <net/mac80211.h>
27 #include <net/codel.h>
28 #include <net/codel_impl.h>
29 #include <asm/unaligned.h>
30 #include <net/fq_impl.h>
31 
32 #include "ieee80211_i.h"
33 #include "driver-ops.h"
34 #include "led.h"
35 #include "mesh.h"
36 #include "wep.h"
37 #include "wpa.h"
38 #include "wme.h"
39 #include "rate.h"
40 
41 /* misc utils */
42 
43 static inline void ieee80211_tx_stats(struct net_device *dev, u32 len)
44 {
45 	struct pcpu_sw_netstats *tstats = this_cpu_ptr(dev->tstats);
46 
47 	u64_stats_update_begin(&tstats->syncp);
48 	tstats->tx_packets++;
49 	tstats->tx_bytes += len;
50 	u64_stats_update_end(&tstats->syncp);
51 }
52 
53 static __le16 ieee80211_duration(struct ieee80211_tx_data *tx,
54 				 struct sk_buff *skb, int group_addr,
55 				 int next_frag_len)
56 {
57 	int rate, mrate, erp, dur, i, shift = 0;
58 	struct ieee80211_rate *txrate;
59 	struct ieee80211_local *local = tx->local;
60 	struct ieee80211_supported_band *sband;
61 	struct ieee80211_hdr *hdr;
62 	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
63 	struct ieee80211_chanctx_conf *chanctx_conf;
64 	u32 rate_flags = 0;
65 
66 	rcu_read_lock();
67 	chanctx_conf = rcu_dereference(tx->sdata->vif.chanctx_conf);
68 	if (chanctx_conf) {
69 		shift = ieee80211_chandef_get_shift(&chanctx_conf->def);
70 		rate_flags = ieee80211_chandef_rate_flags(&chanctx_conf->def);
71 	}
72 	rcu_read_unlock();
73 
74 	/* assume HW handles this */
75 	if (tx->rate.flags & (IEEE80211_TX_RC_MCS | IEEE80211_TX_RC_VHT_MCS))
76 		return 0;
77 
78 	/* uh huh? */
79 	if (WARN_ON_ONCE(tx->rate.idx < 0))
80 		return 0;
81 
82 	sband = local->hw.wiphy->bands[info->band];
83 	txrate = &sband->bitrates[tx->rate.idx];
84 
85 	erp = txrate->flags & IEEE80211_RATE_ERP_G;
86 
87 	/*
88 	 * data and mgmt (except PS Poll):
89 	 * - during CFP: 32768
90 	 * - during contention period:
91 	 *   if addr1 is group address: 0
92 	 *   if more fragments = 0 and addr1 is individual address: time to
93 	 *      transmit one ACK plus SIFS
94 	 *   if more fragments = 1 and addr1 is individual address: time to
95 	 *      transmit next fragment plus 2 x ACK plus 3 x SIFS
96 	 *
97 	 * IEEE 802.11, 9.6:
98 	 * - control response frame (CTS or ACK) shall be transmitted using the
99 	 *   same rate as the immediately previous frame in the frame exchange
100 	 *   sequence, if this rate belongs to the PHY mandatory rates, or else
101 	 *   at the highest possible rate belonging to the PHY rates in the
102 	 *   BSSBasicRateSet
103 	 */
104 	hdr = (struct ieee80211_hdr *)skb->data;
105 	if (ieee80211_is_ctl(hdr->frame_control)) {
106 		/* TODO: These control frames are not currently sent by
107 		 * mac80211, but should they be implemented, this function
108 		 * needs to be updated to support duration field calculation.
109 		 *
110 		 * RTS: time needed to transmit pending data/mgmt frame plus
111 		 *    one CTS frame plus one ACK frame plus 3 x SIFS
112 		 * CTS: duration of immediately previous RTS minus time
113 		 *    required to transmit CTS and its SIFS
114 		 * ACK: 0 if immediately previous directed data/mgmt had
115 		 *    more=0, with more=1 duration in ACK frame is duration
116 		 *    from previous frame minus time needed to transmit ACK
117 		 *    and its SIFS
118 		 * PS Poll: BIT(15) | BIT(14) | aid
119 		 */
120 		return 0;
121 	}
122 
123 	/* data/mgmt */
124 	if (0 /* FIX: data/mgmt during CFP */)
125 		return cpu_to_le16(32768);
126 
127 	if (group_addr) /* Group address as the destination - no ACK */
128 		return 0;
129 
130 	/* Individual destination address:
131 	 * IEEE 802.11, Ch. 9.6 (after IEEE 802.11g changes)
132 	 * CTS and ACK frames shall be transmitted using the highest rate in
133 	 * basic rate set that is less than or equal to the rate of the
134 	 * immediately previous frame and that is using the same modulation
135 	 * (CCK or OFDM). If no basic rate set matches with these requirements,
136 	 * the highest mandatory rate of the PHY that is less than or equal to
137 	 * the rate of the previous frame is used.
138 	 * Mandatory rates for IEEE 802.11g PHY: 1, 2, 5.5, 11, 6, 12, 24 Mbps
139 	 */
140 	rate = -1;
141 	/* use lowest available if everything fails */
142 	mrate = sband->bitrates[0].bitrate;
143 	for (i = 0; i < sband->n_bitrates; i++) {
144 		struct ieee80211_rate *r = &sband->bitrates[i];
145 
146 		if (r->bitrate > txrate->bitrate)
147 			break;
148 
149 		if ((rate_flags & r->flags) != rate_flags)
150 			continue;
151 
152 		if (tx->sdata->vif.bss_conf.basic_rates & BIT(i))
153 			rate = DIV_ROUND_UP(r->bitrate, 1 << shift);
154 
155 		switch (sband->band) {
156 		case NL80211_BAND_2GHZ: {
157 			u32 flag;
158 			if (tx->sdata->flags & IEEE80211_SDATA_OPERATING_GMODE)
159 				flag = IEEE80211_RATE_MANDATORY_G;
160 			else
161 				flag = IEEE80211_RATE_MANDATORY_B;
162 			if (r->flags & flag)
163 				mrate = r->bitrate;
164 			break;
165 		}
166 		case NL80211_BAND_5GHZ:
167 			if (r->flags & IEEE80211_RATE_MANDATORY_A)
168 				mrate = r->bitrate;
169 			break;
170 		case NL80211_BAND_60GHZ:
171 			/* TODO, for now fall through */
172 		case NUM_NL80211_BANDS:
173 			WARN_ON(1);
174 			break;
175 		}
176 	}
177 	if (rate == -1) {
178 		/* No matching basic rate found; use highest suitable mandatory
179 		 * PHY rate */
180 		rate = DIV_ROUND_UP(mrate, 1 << shift);
181 	}
182 
183 	/* Don't calculate ACKs for QoS Frames with NoAck Policy set */
184 	if (ieee80211_is_data_qos(hdr->frame_control) &&
185 	    *(ieee80211_get_qos_ctl(hdr)) & IEEE80211_QOS_CTL_ACK_POLICY_NOACK)
186 		dur = 0;
187 	else
188 		/* Time needed to transmit ACK
189 		 * (10 bytes + 4-byte FCS = 112 bits) plus SIFS; rounded up
190 		 * to closest integer */
191 		dur = ieee80211_frame_duration(sband->band, 10, rate, erp,
192 				tx->sdata->vif.bss_conf.use_short_preamble,
193 				shift);
194 
195 	if (next_frag_len) {
196 		/* Frame is fragmented: duration increases with time needed to
197 		 * transmit next fragment plus ACK and 2 x SIFS. */
198 		dur *= 2; /* ACK + SIFS */
199 		/* next fragment */
200 		dur += ieee80211_frame_duration(sband->band, next_frag_len,
201 				txrate->bitrate, erp,
202 				tx->sdata->vif.bss_conf.use_short_preamble,
203 				shift);
204 	}
205 
206 	return cpu_to_le16(dur);
207 }
208 
209 /* tx handlers */
210 static ieee80211_tx_result debug_noinline
211 ieee80211_tx_h_dynamic_ps(struct ieee80211_tx_data *tx)
212 {
213 	struct ieee80211_local *local = tx->local;
214 	struct ieee80211_if_managed *ifmgd;
215 
216 	/* driver doesn't support power save */
217 	if (!ieee80211_hw_check(&local->hw, SUPPORTS_PS))
218 		return TX_CONTINUE;
219 
220 	/* hardware does dynamic power save */
221 	if (ieee80211_hw_check(&local->hw, SUPPORTS_DYNAMIC_PS))
222 		return TX_CONTINUE;
223 
224 	/* dynamic power save disabled */
225 	if (local->hw.conf.dynamic_ps_timeout <= 0)
226 		return TX_CONTINUE;
227 
228 	/* we are scanning, don't enable power save */
229 	if (local->scanning)
230 		return TX_CONTINUE;
231 
232 	if (!local->ps_sdata)
233 		return TX_CONTINUE;
234 
235 	/* No point if we're going to suspend */
236 	if (local->quiescing)
237 		return TX_CONTINUE;
238 
239 	/* dynamic ps is supported only in managed mode */
240 	if (tx->sdata->vif.type != NL80211_IFTYPE_STATION)
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
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_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->sdata->vif.type == NL80211_IFTYPE_WDS)
315 		return TX_CONTINUE;
316 
317 	if (tx->flags & IEEE80211_TX_PS_BUFFERED)
318 		return TX_CONTINUE;
319 
320 	if (tx->sta)
321 		assoc = test_sta_flag(tx->sta, WLAN_STA_ASSOC);
322 
323 	if (likely(tx->flags & IEEE80211_TX_UNICAST)) {
324 		if (unlikely(!assoc &&
325 			     ieee80211_is_data(hdr->frame_control))) {
326 #ifdef CONFIG_MAC80211_VERBOSE_DEBUG
327 			sdata_info(tx->sdata,
328 				   "dropped data frame to not associated station %pM\n",
329 				   hdr->addr1);
330 #endif
331 			I802_DEBUG_INC(tx->local->tx_handlers_drop_not_assoc);
332 			return TX_DROP;
333 		}
334 	} else if (unlikely(tx->sdata->vif.type == NL80211_IFTYPE_AP &&
335 			    ieee80211_is_data(hdr->frame_control) &&
336 			    !atomic_read(&tx->sdata->u.ap.num_mcast_sta))) {
337 		/*
338 		 * No associated STAs - no need to send multicast
339 		 * frames.
340 		 */
341 		return TX_DROP;
342 	}
343 
344 	return TX_CONTINUE;
345 }
346 
347 /* This function is called whenever the AP is about to exceed the maximum limit
348  * of buffered frames for power saving STAs. This situation should not really
349  * happen often during normal operation, so dropping the oldest buffered packet
350  * from each queue should be OK to make some room for new frames. */
351 static void purge_old_ps_buffers(struct ieee80211_local *local)
352 {
353 	int total = 0, purged = 0;
354 	struct sk_buff *skb;
355 	struct ieee80211_sub_if_data *sdata;
356 	struct sta_info *sta;
357 
358 	list_for_each_entry_rcu(sdata, &local->interfaces, list) {
359 		struct ps_data *ps;
360 
361 		if (sdata->vif.type == NL80211_IFTYPE_AP)
362 			ps = &sdata->u.ap.ps;
363 		else if (ieee80211_vif_is_mesh(&sdata->vif))
364 			ps = &sdata->u.mesh.ps;
365 		else
366 			continue;
367 
368 		skb = skb_dequeue(&ps->bc_buf);
369 		if (skb) {
370 			purged++;
371 			ieee80211_free_txskb(&local->hw, skb);
372 		}
373 		total += skb_queue_len(&ps->bc_buf);
374 	}
375 
376 	/*
377 	 * Drop one frame from each station from the lowest-priority
378 	 * AC that has frames at all.
379 	 */
380 	list_for_each_entry_rcu(sta, &local->sta_list, list) {
381 		int ac;
382 
383 		for (ac = IEEE80211_AC_BK; ac >= IEEE80211_AC_VO; ac--) {
384 			skb = skb_dequeue(&sta->ps_tx_buf[ac]);
385 			total += skb_queue_len(&sta->ps_tx_buf[ac]);
386 			if (skb) {
387 				purged++;
388 				ieee80211_free_txskb(&local->hw, skb);
389 				break;
390 			}
391 		}
392 	}
393 
394 	local->total_ps_buffered = total;
395 	ps_dbg_hw(&local->hw, "PS buffers full - purged %d frames\n", purged);
396 }
397 
398 static ieee80211_tx_result
399 ieee80211_tx_h_multicast_ps_buf(struct ieee80211_tx_data *tx)
400 {
401 	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(tx->skb);
402 	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)tx->skb->data;
403 	struct ps_data *ps;
404 
405 	/*
406 	 * broadcast/multicast frame
407 	 *
408 	 * If any of the associated/peer stations is in power save mode,
409 	 * the frame is buffered to be sent after DTIM beacon frame.
410 	 * This is done either by the hardware or us.
411 	 */
412 
413 	/* powersaving STAs currently only in AP/VLAN/mesh mode */
414 	if (tx->sdata->vif.type == NL80211_IFTYPE_AP ||
415 	    tx->sdata->vif.type == NL80211_IFTYPE_AP_VLAN) {
416 		if (!tx->sdata->bss)
417 			return TX_CONTINUE;
418 
419 		ps = &tx->sdata->bss->ps;
420 	} else if (ieee80211_vif_is_mesh(&tx->sdata->vif)) {
421 		ps = &tx->sdata->u.mesh.ps;
422 	} else {
423 		return TX_CONTINUE;
424 	}
425 
426 
427 	/* no buffering for ordered frames */
428 	if (ieee80211_has_order(hdr->frame_control))
429 		return TX_CONTINUE;
430 
431 	if (ieee80211_is_probe_req(hdr->frame_control))
432 		return TX_CONTINUE;
433 
434 	if (ieee80211_hw_check(&tx->local->hw, QUEUE_CONTROL))
435 		info->hw_queue = tx->sdata->vif.cab_queue;
436 
437 	/* no stations in PS mode */
438 	if (!atomic_read(&ps->num_sta_ps))
439 		return TX_CONTINUE;
440 
441 	info->flags |= IEEE80211_TX_CTL_SEND_AFTER_DTIM;
442 
443 	/* device releases frame after DTIM beacon */
444 	if (!ieee80211_hw_check(&tx->local->hw, HOST_BROADCAST_PS_BUFFERING))
445 		return TX_CONTINUE;
446 
447 	/* buffered in mac80211 */
448 	if (tx->local->total_ps_buffered >= TOTAL_MAX_TX_BUFFER)
449 		purge_old_ps_buffers(tx->local);
450 
451 	if (skb_queue_len(&ps->bc_buf) >= AP_MAX_BC_BUFFER) {
452 		ps_dbg(tx->sdata,
453 		       "BC TX buffer full - dropping the oldest frame\n");
454 		ieee80211_free_txskb(&tx->local->hw, skb_dequeue(&ps->bc_buf));
455 	} else
456 		tx->local->total_ps_buffered++;
457 
458 	skb_queue_tail(&ps->bc_buf, tx->skb);
459 
460 	return TX_QUEUED;
461 }
462 
463 static int ieee80211_use_mfp(__le16 fc, struct sta_info *sta,
464 			     struct sk_buff *skb)
465 {
466 	if (!ieee80211_is_mgmt(fc))
467 		return 0;
468 
469 	if (sta == NULL || !test_sta_flag(sta, WLAN_STA_MFP))
470 		return 0;
471 
472 	if (!ieee80211_is_robust_mgmt_frame(skb))
473 		return 0;
474 
475 	return 1;
476 }
477 
478 static ieee80211_tx_result
479 ieee80211_tx_h_unicast_ps_buf(struct ieee80211_tx_data *tx)
480 {
481 	struct sta_info *sta = tx->sta;
482 	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(tx->skb);
483 	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)tx->skb->data;
484 	struct ieee80211_local *local = tx->local;
485 
486 	if (unlikely(!sta))
487 		return TX_CONTINUE;
488 
489 	if (unlikely((test_sta_flag(sta, WLAN_STA_PS_STA) ||
490 		      test_sta_flag(sta, WLAN_STA_PS_DRIVER) ||
491 		      test_sta_flag(sta, WLAN_STA_PS_DELIVER)) &&
492 		     !(info->flags & IEEE80211_TX_CTL_NO_PS_BUFFER))) {
493 		int ac = skb_get_queue_mapping(tx->skb);
494 
495 		if (ieee80211_is_mgmt(hdr->frame_control) &&
496 		    !ieee80211_is_bufferable_mmpdu(hdr->frame_control)) {
497 			info->flags |= IEEE80211_TX_CTL_NO_PS_BUFFER;
498 			return TX_CONTINUE;
499 		}
500 
501 		ps_dbg(sta->sdata, "STA %pM aid %d: PS buffer for AC %d\n",
502 		       sta->sta.addr, sta->sta.aid, ac);
503 		if (tx->local->total_ps_buffered >= TOTAL_MAX_TX_BUFFER)
504 			purge_old_ps_buffers(tx->local);
505 
506 		/* sync with ieee80211_sta_ps_deliver_wakeup */
507 		spin_lock(&sta->ps_lock);
508 		/*
509 		 * STA woke up the meantime and all the frames on ps_tx_buf have
510 		 * been queued to pending queue. No reordering can happen, go
511 		 * ahead and Tx the packet.
512 		 */
513 		if (!test_sta_flag(sta, WLAN_STA_PS_STA) &&
514 		    !test_sta_flag(sta, WLAN_STA_PS_DRIVER) &&
515 		    !test_sta_flag(sta, WLAN_STA_PS_DELIVER)) {
516 			spin_unlock(&sta->ps_lock);
517 			return TX_CONTINUE;
518 		}
519 
520 		if (skb_queue_len(&sta->ps_tx_buf[ac]) >= STA_MAX_TX_BUFFER) {
521 			struct sk_buff *old = skb_dequeue(&sta->ps_tx_buf[ac]);
522 			ps_dbg(tx->sdata,
523 			       "STA %pM TX buffer for AC %d full - dropping oldest frame\n",
524 			       sta->sta.addr, ac);
525 			ieee80211_free_txskb(&local->hw, old);
526 		} else
527 			tx->local->total_ps_buffered++;
528 
529 		info->control.jiffies = jiffies;
530 		info->control.vif = &tx->sdata->vif;
531 		info->flags |= IEEE80211_TX_INTFL_NEED_TXPROCESSING;
532 		info->flags &= ~IEEE80211_TX_TEMPORARY_FLAGS;
533 		skb_queue_tail(&sta->ps_tx_buf[ac], tx->skb);
534 		spin_unlock(&sta->ps_lock);
535 
536 		if (!timer_pending(&local->sta_cleanup))
537 			mod_timer(&local->sta_cleanup,
538 				  round_jiffies(jiffies +
539 						STA_INFO_CLEANUP_INTERVAL));
540 
541 		/*
542 		 * We queued up some frames, so the TIM bit might
543 		 * need to be set, recalculate it.
544 		 */
545 		sta_info_recalc_tim(sta);
546 
547 		return TX_QUEUED;
548 	} else if (unlikely(test_sta_flag(sta, WLAN_STA_PS_STA))) {
549 		ps_dbg(tx->sdata,
550 		       "STA %pM in PS mode, but polling/in SP -> send frame\n",
551 		       sta->sta.addr);
552 	}
553 
554 	return TX_CONTINUE;
555 }
556 
557 static ieee80211_tx_result debug_noinline
558 ieee80211_tx_h_ps_buf(struct ieee80211_tx_data *tx)
559 {
560 	if (unlikely(tx->flags & IEEE80211_TX_PS_BUFFERED))
561 		return TX_CONTINUE;
562 
563 	if (tx->flags & IEEE80211_TX_UNICAST)
564 		return ieee80211_tx_h_unicast_ps_buf(tx);
565 	else
566 		return ieee80211_tx_h_multicast_ps_buf(tx);
567 }
568 
569 static ieee80211_tx_result debug_noinline
570 ieee80211_tx_h_check_control_port_protocol(struct ieee80211_tx_data *tx)
571 {
572 	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(tx->skb);
573 
574 	if (unlikely(tx->sdata->control_port_protocol == tx->skb->protocol)) {
575 		if (tx->sdata->control_port_no_encrypt)
576 			info->flags |= IEEE80211_TX_INTFL_DONT_ENCRYPT;
577 		info->control.flags |= IEEE80211_TX_CTRL_PORT_CTRL_PROTO;
578 		info->flags |= IEEE80211_TX_CTL_USE_MINRATE;
579 	}
580 
581 	return TX_CONTINUE;
582 }
583 
584 static ieee80211_tx_result debug_noinline
585 ieee80211_tx_h_select_key(struct ieee80211_tx_data *tx)
586 {
587 	struct ieee80211_key *key;
588 	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(tx->skb);
589 	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)tx->skb->data;
590 
591 	if (unlikely(info->flags & IEEE80211_TX_INTFL_DONT_ENCRYPT))
592 		tx->key = NULL;
593 	else if (tx->sta &&
594 		 (key = rcu_dereference(tx->sta->ptk[tx->sta->ptk_idx])))
595 		tx->key = key;
596 	else if (ieee80211_is_group_privacy_action(tx->skb) &&
597 		(key = rcu_dereference(tx->sdata->default_multicast_key)))
598 		tx->key = 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 		 (key = rcu_dereference(tx->sdata->default_mgmt_key)))
603 		tx->key = key;
604 	else if (is_multicast_ether_addr(hdr->addr1) &&
605 		 (key = rcu_dereference(tx->sdata->default_multicast_key)))
606 		tx->key = key;
607 	else if (!is_multicast_ether_addr(hdr->addr1) &&
608 		 (key = rcu_dereference(tx->sdata->default_unicast_key)))
609 		tx->key = key;
610 	else
611 		tx->key = NULL;
612 
613 	if (tx->key) {
614 		bool skip_hw = false;
615 
616 		/* TODO: add threshold stuff again */
617 
618 		switch (tx->key->conf.cipher) {
619 		case WLAN_CIPHER_SUITE_WEP40:
620 		case WLAN_CIPHER_SUITE_WEP104:
621 		case WLAN_CIPHER_SUITE_TKIP:
622 			if (!ieee80211_is_data_present(hdr->frame_control))
623 				tx->key = NULL;
624 			break;
625 		case WLAN_CIPHER_SUITE_CCMP:
626 		case WLAN_CIPHER_SUITE_CCMP_256:
627 		case WLAN_CIPHER_SUITE_GCMP:
628 		case WLAN_CIPHER_SUITE_GCMP_256:
629 			if (!ieee80211_is_data_present(hdr->frame_control) &&
630 			    !ieee80211_use_mfp(hdr->frame_control, tx->sta,
631 					       tx->skb) &&
632 			    !ieee80211_is_group_privacy_action(tx->skb))
633 				tx->key = NULL;
634 			else
635 				skip_hw = (tx->key->conf.flags &
636 					   IEEE80211_KEY_FLAG_SW_MGMT_TX) &&
637 					ieee80211_is_mgmt(hdr->frame_control);
638 			break;
639 		case WLAN_CIPHER_SUITE_AES_CMAC:
640 		case WLAN_CIPHER_SUITE_BIP_CMAC_256:
641 		case WLAN_CIPHER_SUITE_BIP_GMAC_128:
642 		case WLAN_CIPHER_SUITE_BIP_GMAC_256:
643 			if (!ieee80211_is_mgmt(hdr->frame_control))
644 				tx->key = NULL;
645 			break;
646 		}
647 
648 		if (unlikely(tx->key && tx->key->flags & KEY_FLAG_TAINTED &&
649 			     !ieee80211_is_deauth(hdr->frame_control)))
650 			return TX_DROP;
651 
652 		if (!skip_hw && tx->key &&
653 		    tx->key->flags & KEY_FLAG_UPLOADED_TO_HARDWARE)
654 			info->control.hw_key = &tx->key->conf;
655 	}
656 
657 	return TX_CONTINUE;
658 }
659 
660 static ieee80211_tx_result debug_noinline
661 ieee80211_tx_h_rate_ctrl(struct ieee80211_tx_data *tx)
662 {
663 	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(tx->skb);
664 	struct ieee80211_hdr *hdr = (void *)tx->skb->data;
665 	struct ieee80211_supported_band *sband;
666 	u32 len;
667 	struct ieee80211_tx_rate_control txrc;
668 	struct ieee80211_sta_rates *ratetbl = NULL;
669 	bool assoc = false;
670 
671 	memset(&txrc, 0, sizeof(txrc));
672 
673 	sband = tx->local->hw.wiphy->bands[info->band];
674 
675 	len = min_t(u32, tx->skb->len + FCS_LEN,
676 			 tx->local->hw.wiphy->frag_threshold);
677 
678 	/* set up the tx rate control struct we give the RC algo */
679 	txrc.hw = &tx->local->hw;
680 	txrc.sband = sband;
681 	txrc.bss_conf = &tx->sdata->vif.bss_conf;
682 	txrc.skb = tx->skb;
683 	txrc.reported_rate.idx = -1;
684 	txrc.rate_idx_mask = tx->sdata->rc_rateidx_mask[info->band];
685 	if (txrc.rate_idx_mask == (1 << sband->n_bitrates) - 1)
686 		txrc.max_rate_idx = -1;
687 	else
688 		txrc.max_rate_idx = fls(txrc.rate_idx_mask) - 1;
689 
690 	if (tx->sdata->rc_has_mcs_mask[info->band])
691 		txrc.rate_idx_mcs_mask =
692 			tx->sdata->rc_rateidx_mcs_mask[info->band];
693 
694 	txrc.bss = (tx->sdata->vif.type == NL80211_IFTYPE_AP ||
695 		    tx->sdata->vif.type == NL80211_IFTYPE_MESH_POINT ||
696 		    tx->sdata->vif.type == NL80211_IFTYPE_ADHOC ||
697 		    tx->sdata->vif.type == NL80211_IFTYPE_OCB);
698 
699 	/* set up RTS protection if desired */
700 	if (len > tx->local->hw.wiphy->rts_threshold) {
701 		txrc.rts = true;
702 	}
703 
704 	info->control.use_rts = txrc.rts;
705 	info->control.use_cts_prot = tx->sdata->vif.bss_conf.use_cts_prot;
706 
707 	/*
708 	 * Use short preamble if the BSS can handle it, but not for
709 	 * management frames unless we know the receiver can handle
710 	 * that -- the management frame might be to a station that
711 	 * just wants a probe response.
712 	 */
713 	if (tx->sdata->vif.bss_conf.use_short_preamble &&
714 	    (ieee80211_is_data(hdr->frame_control) ||
715 	     (tx->sta && test_sta_flag(tx->sta, WLAN_STA_SHORT_PREAMBLE))))
716 		txrc.short_preamble = true;
717 
718 	info->control.short_preamble = txrc.short_preamble;
719 
720 	/* don't ask rate control when rate already injected via radiotap */
721 	if (info->control.flags & IEEE80211_TX_CTRL_RATE_INJECT)
722 		return TX_CONTINUE;
723 
724 	if (tx->sta)
725 		assoc = test_sta_flag(tx->sta, WLAN_STA_ASSOC);
726 
727 	/*
728 	 * Lets not bother rate control if we're associated and cannot
729 	 * talk to the sta. This should not happen.
730 	 */
731 	if (WARN(test_bit(SCAN_SW_SCANNING, &tx->local->scanning) && assoc &&
732 		 !rate_usable_index_exists(sband, &tx->sta->sta),
733 		 "%s: Dropped data frame as no usable bitrate found while "
734 		 "scanning and associated. Target station: "
735 		 "%pM on %d GHz band\n",
736 		 tx->sdata->name, hdr->addr1,
737 		 info->band ? 5 : 2))
738 		return TX_DROP;
739 
740 	/*
741 	 * If we're associated with the sta at this point we know we can at
742 	 * least send the frame at the lowest bit rate.
743 	 */
744 	rate_control_get_rate(tx->sdata, tx->sta, &txrc);
745 
746 	if (tx->sta && !info->control.skip_table)
747 		ratetbl = rcu_dereference(tx->sta->sta.rates);
748 
749 	if (unlikely(info->control.rates[0].idx < 0)) {
750 		if (ratetbl) {
751 			struct ieee80211_tx_rate rate = {
752 				.idx = ratetbl->rate[0].idx,
753 				.flags = ratetbl->rate[0].flags,
754 				.count = ratetbl->rate[0].count
755 			};
756 
757 			if (ratetbl->rate[0].idx < 0)
758 				return TX_DROP;
759 
760 			tx->rate = rate;
761 		} else {
762 			return TX_DROP;
763 		}
764 	} else {
765 		tx->rate = info->control.rates[0];
766 	}
767 
768 	if (txrc.reported_rate.idx < 0) {
769 		txrc.reported_rate = tx->rate;
770 		if (tx->sta && ieee80211_is_data(hdr->frame_control))
771 			tx->sta->tx_stats.last_rate = txrc.reported_rate;
772 	} else if (tx->sta)
773 		tx->sta->tx_stats.last_rate = txrc.reported_rate;
774 
775 	if (ratetbl)
776 		return TX_CONTINUE;
777 
778 	if (unlikely(!info->control.rates[0].count))
779 		info->control.rates[0].count = 1;
780 
781 	if (WARN_ON_ONCE((info->control.rates[0].count > 1) &&
782 			 (info->flags & IEEE80211_TX_CTL_NO_ACK)))
783 		info->control.rates[0].count = 1;
784 
785 	return TX_CONTINUE;
786 }
787 
788 static __le16 ieee80211_tx_next_seq(struct sta_info *sta, int tid)
789 {
790 	u16 *seq = &sta->tid_seq[tid];
791 	__le16 ret = cpu_to_le16(*seq);
792 
793 	/* Increase the sequence number. */
794 	*seq = (*seq + 0x10) & IEEE80211_SCTL_SEQ;
795 
796 	return ret;
797 }
798 
799 static ieee80211_tx_result debug_noinline
800 ieee80211_tx_h_sequence(struct ieee80211_tx_data *tx)
801 {
802 	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(tx->skb);
803 	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)tx->skb->data;
804 	u8 *qc;
805 	int tid;
806 
807 	/*
808 	 * Packet injection may want to control the sequence
809 	 * number, if we have no matching interface then we
810 	 * neither assign one ourselves nor ask the driver to.
811 	 */
812 	if (unlikely(info->control.vif->type == NL80211_IFTYPE_MONITOR))
813 		return TX_CONTINUE;
814 
815 	if (unlikely(ieee80211_is_ctl(hdr->frame_control)))
816 		return TX_CONTINUE;
817 
818 	if (ieee80211_hdrlen(hdr->frame_control) < 24)
819 		return TX_CONTINUE;
820 
821 	if (ieee80211_is_qos_nullfunc(hdr->frame_control))
822 		return TX_CONTINUE;
823 
824 	/*
825 	 * Anything but QoS data that has a sequence number field
826 	 * (is long enough) gets a sequence number from the global
827 	 * counter.  QoS data frames with a multicast destination
828 	 * also use the global counter (802.11-2012 9.3.2.10).
829 	 */
830 	if (!ieee80211_is_data_qos(hdr->frame_control) ||
831 	    is_multicast_ether_addr(hdr->addr1)) {
832 		/* driver should assign sequence number */
833 		info->flags |= IEEE80211_TX_CTL_ASSIGN_SEQ;
834 		/* for pure STA mode without beacons, we can do it */
835 		hdr->seq_ctrl = cpu_to_le16(tx->sdata->sequence_number);
836 		tx->sdata->sequence_number += 0x10;
837 		if (tx->sta)
838 			tx->sta->tx_stats.msdu[IEEE80211_NUM_TIDS]++;
839 		return TX_CONTINUE;
840 	}
841 
842 	/*
843 	 * This should be true for injected/management frames only, for
844 	 * management frames we have set the IEEE80211_TX_CTL_ASSIGN_SEQ
845 	 * above since they are not QoS-data frames.
846 	 */
847 	if (!tx->sta)
848 		return TX_CONTINUE;
849 
850 	/* include per-STA, per-TID sequence counter */
851 
852 	qc = ieee80211_get_qos_ctl(hdr);
853 	tid = *qc & IEEE80211_QOS_CTL_TID_MASK;
854 	tx->sta->tx_stats.msdu[tid]++;
855 
856 	hdr->seq_ctrl = ieee80211_tx_next_seq(tx->sta, tid);
857 
858 	return TX_CONTINUE;
859 }
860 
861 static int ieee80211_fragment(struct ieee80211_tx_data *tx,
862 			      struct sk_buff *skb, int hdrlen,
863 			      int frag_threshold)
864 {
865 	struct ieee80211_local *local = tx->local;
866 	struct ieee80211_tx_info *info;
867 	struct sk_buff *tmp;
868 	int per_fragm = frag_threshold - hdrlen - FCS_LEN;
869 	int pos = hdrlen + per_fragm;
870 	int rem = skb->len - hdrlen - per_fragm;
871 
872 	if (WARN_ON(rem < 0))
873 		return -EINVAL;
874 
875 	/* first fragment was already added to queue by caller */
876 
877 	while (rem) {
878 		int fraglen = per_fragm;
879 
880 		if (fraglen > rem)
881 			fraglen = rem;
882 		rem -= fraglen;
883 		tmp = dev_alloc_skb(local->tx_headroom +
884 				    frag_threshold +
885 				    tx->sdata->encrypt_headroom +
886 				    IEEE80211_ENCRYPT_TAILROOM);
887 		if (!tmp)
888 			return -ENOMEM;
889 
890 		__skb_queue_tail(&tx->skbs, tmp);
891 
892 		skb_reserve(tmp,
893 			    local->tx_headroom + tx->sdata->encrypt_headroom);
894 
895 		/* copy control information */
896 		memcpy(tmp->cb, skb->cb, sizeof(tmp->cb));
897 
898 		info = IEEE80211_SKB_CB(tmp);
899 		info->flags &= ~(IEEE80211_TX_CTL_CLEAR_PS_FILT |
900 				 IEEE80211_TX_CTL_FIRST_FRAGMENT);
901 
902 		if (rem)
903 			info->flags |= IEEE80211_TX_CTL_MORE_FRAMES;
904 
905 		skb_copy_queue_mapping(tmp, skb);
906 		tmp->priority = skb->priority;
907 		tmp->dev = skb->dev;
908 
909 		/* copy header and data */
910 		memcpy(skb_put(tmp, hdrlen), skb->data, hdrlen);
911 		memcpy(skb_put(tmp, fraglen), skb->data + pos, fraglen);
912 
913 		pos += fraglen;
914 	}
915 
916 	/* adjust first fragment's length */
917 	skb_trim(skb, hdrlen + per_fragm);
918 	return 0;
919 }
920 
921 static ieee80211_tx_result debug_noinline
922 ieee80211_tx_h_fragment(struct ieee80211_tx_data *tx)
923 {
924 	struct sk_buff *skb = tx->skb;
925 	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
926 	struct ieee80211_hdr *hdr = (void *)skb->data;
927 	int frag_threshold = tx->local->hw.wiphy->frag_threshold;
928 	int hdrlen;
929 	int fragnum;
930 
931 	/* no matter what happens, tx->skb moves to tx->skbs */
932 	__skb_queue_tail(&tx->skbs, skb);
933 	tx->skb = NULL;
934 
935 	if (info->flags & IEEE80211_TX_CTL_DONTFRAG)
936 		return TX_CONTINUE;
937 
938 	if (tx->local->ops->set_frag_threshold)
939 		return TX_CONTINUE;
940 
941 	/*
942 	 * Warn when submitting a fragmented A-MPDU frame and drop it.
943 	 * This scenario is handled in ieee80211_tx_prepare but extra
944 	 * caution taken here as fragmented ampdu may cause Tx stop.
945 	 */
946 	if (WARN_ON(info->flags & IEEE80211_TX_CTL_AMPDU))
947 		return TX_DROP;
948 
949 	hdrlen = ieee80211_hdrlen(hdr->frame_control);
950 
951 	/* internal error, why isn't DONTFRAG set? */
952 	if (WARN_ON(skb->len + FCS_LEN <= frag_threshold))
953 		return TX_DROP;
954 
955 	/*
956 	 * Now fragment the frame. This will allocate all the fragments and
957 	 * chain them (using skb as the first fragment) to skb->next.
958 	 * During transmission, we will remove the successfully transmitted
959 	 * fragments from this list. When the low-level driver rejects one
960 	 * of the fragments then we will simply pretend to accept the skb
961 	 * but store it away as pending.
962 	 */
963 	if (ieee80211_fragment(tx, skb, hdrlen, frag_threshold))
964 		return TX_DROP;
965 
966 	/* update duration/seq/flags of fragments */
967 	fragnum = 0;
968 
969 	skb_queue_walk(&tx->skbs, skb) {
970 		const __le16 morefrags = cpu_to_le16(IEEE80211_FCTL_MOREFRAGS);
971 
972 		hdr = (void *)skb->data;
973 		info = IEEE80211_SKB_CB(skb);
974 
975 		if (!skb_queue_is_last(&tx->skbs, skb)) {
976 			hdr->frame_control |= morefrags;
977 			/*
978 			 * No multi-rate retries for fragmented frames, that
979 			 * would completely throw off the NAV at other STAs.
980 			 */
981 			info->control.rates[1].idx = -1;
982 			info->control.rates[2].idx = -1;
983 			info->control.rates[3].idx = -1;
984 			BUILD_BUG_ON(IEEE80211_TX_MAX_RATES != 4);
985 			info->flags &= ~IEEE80211_TX_CTL_RATE_CTRL_PROBE;
986 		} else {
987 			hdr->frame_control &= ~morefrags;
988 		}
989 		hdr->seq_ctrl |= cpu_to_le16(fragnum & IEEE80211_SCTL_FRAG);
990 		fragnum++;
991 	}
992 
993 	return TX_CONTINUE;
994 }
995 
996 static ieee80211_tx_result debug_noinline
997 ieee80211_tx_h_stats(struct ieee80211_tx_data *tx)
998 {
999 	struct sk_buff *skb;
1000 	int ac = -1;
1001 
1002 	if (!tx->sta)
1003 		return TX_CONTINUE;
1004 
1005 	skb_queue_walk(&tx->skbs, skb) {
1006 		ac = skb_get_queue_mapping(skb);
1007 		tx->sta->tx_stats.bytes[ac] += skb->len;
1008 	}
1009 	if (ac >= 0)
1010 		tx->sta->tx_stats.packets[ac]++;
1011 
1012 	return TX_CONTINUE;
1013 }
1014 
1015 static ieee80211_tx_result debug_noinline
1016 ieee80211_tx_h_encrypt(struct ieee80211_tx_data *tx)
1017 {
1018 	if (!tx->key)
1019 		return TX_CONTINUE;
1020 
1021 	switch (tx->key->conf.cipher) {
1022 	case WLAN_CIPHER_SUITE_WEP40:
1023 	case WLAN_CIPHER_SUITE_WEP104:
1024 		return ieee80211_crypto_wep_encrypt(tx);
1025 	case WLAN_CIPHER_SUITE_TKIP:
1026 		return ieee80211_crypto_tkip_encrypt(tx);
1027 	case WLAN_CIPHER_SUITE_CCMP:
1028 		return ieee80211_crypto_ccmp_encrypt(
1029 			tx, IEEE80211_CCMP_MIC_LEN);
1030 	case WLAN_CIPHER_SUITE_CCMP_256:
1031 		return ieee80211_crypto_ccmp_encrypt(
1032 			tx, IEEE80211_CCMP_256_MIC_LEN);
1033 	case WLAN_CIPHER_SUITE_AES_CMAC:
1034 		return ieee80211_crypto_aes_cmac_encrypt(tx);
1035 	case WLAN_CIPHER_SUITE_BIP_CMAC_256:
1036 		return ieee80211_crypto_aes_cmac_256_encrypt(tx);
1037 	case WLAN_CIPHER_SUITE_BIP_GMAC_128:
1038 	case WLAN_CIPHER_SUITE_BIP_GMAC_256:
1039 		return ieee80211_crypto_aes_gmac_encrypt(tx);
1040 	case WLAN_CIPHER_SUITE_GCMP:
1041 	case WLAN_CIPHER_SUITE_GCMP_256:
1042 		return ieee80211_crypto_gcmp_encrypt(tx);
1043 	default:
1044 		return ieee80211_crypto_hw_encrypt(tx);
1045 	}
1046 
1047 	return TX_DROP;
1048 }
1049 
1050 static ieee80211_tx_result debug_noinline
1051 ieee80211_tx_h_calculate_duration(struct ieee80211_tx_data *tx)
1052 {
1053 	struct sk_buff *skb;
1054 	struct ieee80211_hdr *hdr;
1055 	int next_len;
1056 	bool group_addr;
1057 
1058 	skb_queue_walk(&tx->skbs, skb) {
1059 		hdr = (void *) skb->data;
1060 		if (unlikely(ieee80211_is_pspoll(hdr->frame_control)))
1061 			break; /* must not overwrite AID */
1062 		if (!skb_queue_is_last(&tx->skbs, skb)) {
1063 			struct sk_buff *next = skb_queue_next(&tx->skbs, skb);
1064 			next_len = next->len;
1065 		} else
1066 			next_len = 0;
1067 		group_addr = is_multicast_ether_addr(hdr->addr1);
1068 
1069 		hdr->duration_id =
1070 			ieee80211_duration(tx, skb, group_addr, next_len);
1071 	}
1072 
1073 	return TX_CONTINUE;
1074 }
1075 
1076 /* actual transmit path */
1077 
1078 static bool ieee80211_tx_prep_agg(struct ieee80211_tx_data *tx,
1079 				  struct sk_buff *skb,
1080 				  struct ieee80211_tx_info *info,
1081 				  struct tid_ampdu_tx *tid_tx,
1082 				  int tid)
1083 {
1084 	bool queued = false;
1085 	bool reset_agg_timer = false;
1086 	struct sk_buff *purge_skb = NULL;
1087 
1088 	if (test_bit(HT_AGG_STATE_OPERATIONAL, &tid_tx->state)) {
1089 		info->flags |= IEEE80211_TX_CTL_AMPDU;
1090 		reset_agg_timer = true;
1091 	} else if (test_bit(HT_AGG_STATE_WANT_START, &tid_tx->state)) {
1092 		/*
1093 		 * nothing -- this aggregation session is being started
1094 		 * but that might still fail with the driver
1095 		 */
1096 	} else if (!tx->sta->sta.txq[tid]) {
1097 		spin_lock(&tx->sta->lock);
1098 		/*
1099 		 * Need to re-check now, because we may get here
1100 		 *
1101 		 *  1) in the window during which the setup is actually
1102 		 *     already done, but not marked yet because not all
1103 		 *     packets are spliced over to the driver pending
1104 		 *     queue yet -- if this happened we acquire the lock
1105 		 *     either before or after the splice happens, but
1106 		 *     need to recheck which of these cases happened.
1107 		 *
1108 		 *  2) during session teardown, if the OPERATIONAL bit
1109 		 *     was cleared due to the teardown but the pointer
1110 		 *     hasn't been assigned NULL yet (or we loaded it
1111 		 *     before it was assigned) -- in this case it may
1112 		 *     now be NULL which means we should just let the
1113 		 *     packet pass through because splicing the frames
1114 		 *     back is already done.
1115 		 */
1116 		tid_tx = rcu_dereference_protected_tid_tx(tx->sta, tid);
1117 
1118 		if (!tid_tx) {
1119 			/* do nothing, let packet pass through */
1120 		} else if (test_bit(HT_AGG_STATE_OPERATIONAL, &tid_tx->state)) {
1121 			info->flags |= IEEE80211_TX_CTL_AMPDU;
1122 			reset_agg_timer = true;
1123 		} else {
1124 			queued = true;
1125 			if (info->flags & IEEE80211_TX_CTL_NO_PS_BUFFER) {
1126 				clear_sta_flag(tx->sta, WLAN_STA_SP);
1127 				ps_dbg(tx->sta->sdata,
1128 				       "STA %pM aid %d: SP frame queued, close the SP w/o telling the peer\n",
1129 				       tx->sta->sta.addr, tx->sta->sta.aid);
1130 			}
1131 			info->control.vif = &tx->sdata->vif;
1132 			info->flags |= IEEE80211_TX_INTFL_NEED_TXPROCESSING;
1133 			info->flags &= ~IEEE80211_TX_TEMPORARY_FLAGS;
1134 			__skb_queue_tail(&tid_tx->pending, skb);
1135 			if (skb_queue_len(&tid_tx->pending) > STA_MAX_TX_BUFFER)
1136 				purge_skb = __skb_dequeue(&tid_tx->pending);
1137 		}
1138 		spin_unlock(&tx->sta->lock);
1139 
1140 		if (purge_skb)
1141 			ieee80211_free_txskb(&tx->local->hw, purge_skb);
1142 	}
1143 
1144 	/* reset session timer */
1145 	if (reset_agg_timer && tid_tx->timeout)
1146 		tid_tx->last_tx = jiffies;
1147 
1148 	return queued;
1149 }
1150 
1151 /*
1152  * initialises @tx
1153  * pass %NULL for the station if unknown, a valid pointer if known
1154  * or an ERR_PTR() if the station is known not to exist
1155  */
1156 static ieee80211_tx_result
1157 ieee80211_tx_prepare(struct ieee80211_sub_if_data *sdata,
1158 		     struct ieee80211_tx_data *tx,
1159 		     struct sta_info *sta, struct sk_buff *skb)
1160 {
1161 	struct ieee80211_local *local = sdata->local;
1162 	struct ieee80211_hdr *hdr;
1163 	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
1164 	int tid;
1165 	u8 *qc;
1166 
1167 	memset(tx, 0, sizeof(*tx));
1168 	tx->skb = skb;
1169 	tx->local = local;
1170 	tx->sdata = sdata;
1171 	__skb_queue_head_init(&tx->skbs);
1172 
1173 	/*
1174 	 * If this flag is set to true anywhere, and we get here,
1175 	 * we are doing the needed processing, so remove the flag
1176 	 * now.
1177 	 */
1178 	info->flags &= ~IEEE80211_TX_INTFL_NEED_TXPROCESSING;
1179 
1180 	hdr = (struct ieee80211_hdr *) skb->data;
1181 
1182 	if (likely(sta)) {
1183 		if (!IS_ERR(sta))
1184 			tx->sta = sta;
1185 	} else {
1186 		if (sdata->vif.type == NL80211_IFTYPE_AP_VLAN) {
1187 			tx->sta = rcu_dereference(sdata->u.vlan.sta);
1188 			if (!tx->sta && sdata->wdev.use_4addr)
1189 				return TX_DROP;
1190 		} else if (info->flags & (IEEE80211_TX_INTFL_NL80211_FRAME_TX |
1191 					  IEEE80211_TX_CTL_INJECTED) ||
1192 			   tx->sdata->control_port_protocol == tx->skb->protocol) {
1193 			tx->sta = sta_info_get_bss(sdata, hdr->addr1);
1194 		}
1195 		if (!tx->sta && !is_multicast_ether_addr(hdr->addr1))
1196 			tx->sta = sta_info_get(sdata, hdr->addr1);
1197 	}
1198 
1199 	if (tx->sta && ieee80211_is_data_qos(hdr->frame_control) &&
1200 	    !ieee80211_is_qos_nullfunc(hdr->frame_control) &&
1201 	    ieee80211_hw_check(&local->hw, AMPDU_AGGREGATION) &&
1202 	    !ieee80211_hw_check(&local->hw, TX_AMPDU_SETUP_IN_HW)) {
1203 		struct tid_ampdu_tx *tid_tx;
1204 
1205 		qc = ieee80211_get_qos_ctl(hdr);
1206 		tid = *qc & IEEE80211_QOS_CTL_TID_MASK;
1207 
1208 		tid_tx = rcu_dereference(tx->sta->ampdu_mlme.tid_tx[tid]);
1209 		if (tid_tx) {
1210 			bool queued;
1211 
1212 			queued = ieee80211_tx_prep_agg(tx, skb, info,
1213 						       tid_tx, tid);
1214 
1215 			if (unlikely(queued))
1216 				return TX_QUEUED;
1217 		}
1218 	}
1219 
1220 	if (is_multicast_ether_addr(hdr->addr1)) {
1221 		tx->flags &= ~IEEE80211_TX_UNICAST;
1222 		info->flags |= IEEE80211_TX_CTL_NO_ACK;
1223 	} else
1224 		tx->flags |= IEEE80211_TX_UNICAST;
1225 
1226 	if (!(info->flags & IEEE80211_TX_CTL_DONTFRAG)) {
1227 		if (!(tx->flags & IEEE80211_TX_UNICAST) ||
1228 		    skb->len + FCS_LEN <= local->hw.wiphy->frag_threshold ||
1229 		    info->flags & IEEE80211_TX_CTL_AMPDU)
1230 			info->flags |= IEEE80211_TX_CTL_DONTFRAG;
1231 	}
1232 
1233 	if (!tx->sta)
1234 		info->flags |= IEEE80211_TX_CTL_CLEAR_PS_FILT;
1235 	else if (test_and_clear_sta_flag(tx->sta, WLAN_STA_CLEAR_PS_FILT)) {
1236 		info->flags |= IEEE80211_TX_CTL_CLEAR_PS_FILT;
1237 		ieee80211_check_fast_xmit(tx->sta);
1238 	}
1239 
1240 	info->flags |= IEEE80211_TX_CTL_FIRST_FRAGMENT;
1241 
1242 	return TX_CONTINUE;
1243 }
1244 
1245 static struct txq_info *ieee80211_get_txq(struct ieee80211_local *local,
1246 					  struct ieee80211_vif *vif,
1247 					  struct ieee80211_sta *pubsta,
1248 					  struct sk_buff *skb)
1249 {
1250 	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
1251 	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
1252 	struct ieee80211_txq *txq = NULL;
1253 
1254 	if ((info->flags & IEEE80211_TX_CTL_SEND_AFTER_DTIM) ||
1255 	    (info->control.flags & IEEE80211_TX_CTRL_PS_RESPONSE))
1256 		return NULL;
1257 
1258 	if (!ieee80211_is_data(hdr->frame_control))
1259 		return NULL;
1260 
1261 	if (pubsta) {
1262 		u8 tid = skb->priority & IEEE80211_QOS_CTL_TID_MASK;
1263 
1264 		txq = pubsta->txq[tid];
1265 	} else if (vif) {
1266 		txq = vif->txq;
1267 	}
1268 
1269 	if (!txq)
1270 		return NULL;
1271 
1272 	return to_txq_info(txq);
1273 }
1274 
1275 static void ieee80211_set_skb_enqueue_time(struct sk_buff *skb)
1276 {
1277 	IEEE80211_SKB_CB(skb)->control.enqueue_time = codel_get_time();
1278 }
1279 
1280 static void ieee80211_set_skb_vif(struct sk_buff *skb, struct txq_info *txqi)
1281 {
1282 	IEEE80211_SKB_CB(skb)->control.vif = txqi->txq.vif;
1283 }
1284 
1285 static u32 codel_skb_len_func(const struct sk_buff *skb)
1286 {
1287 	return skb->len;
1288 }
1289 
1290 static codel_time_t codel_skb_time_func(const struct sk_buff *skb)
1291 {
1292 	const struct ieee80211_tx_info *info;
1293 
1294 	info = (const struct ieee80211_tx_info *)skb->cb;
1295 	return info->control.enqueue_time;
1296 }
1297 
1298 static struct sk_buff *codel_dequeue_func(struct codel_vars *cvars,
1299 					  void *ctx)
1300 {
1301 	struct ieee80211_local *local;
1302 	struct txq_info *txqi;
1303 	struct fq *fq;
1304 	struct fq_flow *flow;
1305 
1306 	txqi = ctx;
1307 	local = vif_to_sdata(txqi->txq.vif)->local;
1308 	fq = &local->fq;
1309 
1310 	if (cvars == &txqi->def_cvars)
1311 		flow = &txqi->def_flow;
1312 	else
1313 		flow = &fq->flows[cvars - local->cvars];
1314 
1315 	return fq_flow_dequeue(fq, flow);
1316 }
1317 
1318 static void codel_drop_func(struct sk_buff *skb,
1319 			    void *ctx)
1320 {
1321 	struct ieee80211_local *local;
1322 	struct ieee80211_hw *hw;
1323 	struct txq_info *txqi;
1324 
1325 	txqi = ctx;
1326 	local = vif_to_sdata(txqi->txq.vif)->local;
1327 	hw = &local->hw;
1328 
1329 	ieee80211_free_txskb(hw, skb);
1330 }
1331 
1332 static struct sk_buff *fq_tin_dequeue_func(struct fq *fq,
1333 					   struct fq_tin *tin,
1334 					   struct fq_flow *flow)
1335 {
1336 	struct ieee80211_local *local;
1337 	struct txq_info *txqi;
1338 	struct codel_vars *cvars;
1339 	struct codel_params *cparams;
1340 	struct codel_stats *cstats;
1341 
1342 	local = container_of(fq, struct ieee80211_local, fq);
1343 	txqi = container_of(tin, struct txq_info, tin);
1344 	cparams = &local->cparams;
1345 	cstats = &txqi->cstats;
1346 
1347 	if (flow == &txqi->def_flow)
1348 		cvars = &txqi->def_cvars;
1349 	else
1350 		cvars = &local->cvars[flow - fq->flows];
1351 
1352 	return codel_dequeue(txqi,
1353 			     &flow->backlog,
1354 			     cparams,
1355 			     cvars,
1356 			     cstats,
1357 			     codel_skb_len_func,
1358 			     codel_skb_time_func,
1359 			     codel_drop_func,
1360 			     codel_dequeue_func);
1361 }
1362 
1363 static void fq_skb_free_func(struct fq *fq,
1364 			     struct fq_tin *tin,
1365 			     struct fq_flow *flow,
1366 			     struct sk_buff *skb)
1367 {
1368 	struct ieee80211_local *local;
1369 
1370 	local = container_of(fq, struct ieee80211_local, fq);
1371 	ieee80211_free_txskb(&local->hw, skb);
1372 }
1373 
1374 static struct fq_flow *fq_flow_get_default_func(struct fq *fq,
1375 						struct fq_tin *tin,
1376 						int idx,
1377 						struct sk_buff *skb)
1378 {
1379 	struct txq_info *txqi;
1380 
1381 	txqi = container_of(tin, struct txq_info, tin);
1382 	return &txqi->def_flow;
1383 }
1384 
1385 static void ieee80211_txq_enqueue(struct ieee80211_local *local,
1386 				  struct txq_info *txqi,
1387 				  struct sk_buff *skb)
1388 {
1389 	struct fq *fq = &local->fq;
1390 	struct fq_tin *tin = &txqi->tin;
1391 
1392 	ieee80211_set_skb_enqueue_time(skb);
1393 	fq_tin_enqueue(fq, tin, skb,
1394 		       fq_skb_free_func,
1395 		       fq_flow_get_default_func);
1396 }
1397 
1398 void ieee80211_txq_init(struct ieee80211_sub_if_data *sdata,
1399 			struct sta_info *sta,
1400 			struct txq_info *txqi, int tid)
1401 {
1402 	fq_tin_init(&txqi->tin);
1403 	fq_flow_init(&txqi->def_flow);
1404 	codel_vars_init(&txqi->def_cvars);
1405 	codel_stats_init(&txqi->cstats);
1406 	__skb_queue_head_init(&txqi->frags);
1407 
1408 	txqi->txq.vif = &sdata->vif;
1409 
1410 	if (sta) {
1411 		txqi->txq.sta = &sta->sta;
1412 		sta->sta.txq[tid] = &txqi->txq;
1413 		txqi->txq.tid = tid;
1414 		txqi->txq.ac = ieee802_1d_to_ac[tid & 7];
1415 	} else {
1416 		sdata->vif.txq = &txqi->txq;
1417 		txqi->txq.tid = 0;
1418 		txqi->txq.ac = IEEE80211_AC_BE;
1419 	}
1420 }
1421 
1422 void ieee80211_txq_purge(struct ieee80211_local *local,
1423 			 struct txq_info *txqi)
1424 {
1425 	struct fq *fq = &local->fq;
1426 	struct fq_tin *tin = &txqi->tin;
1427 
1428 	fq_tin_reset(fq, tin, fq_skb_free_func);
1429 	ieee80211_purge_tx_queue(&local->hw, &txqi->frags);
1430 }
1431 
1432 int ieee80211_txq_setup_flows(struct ieee80211_local *local)
1433 {
1434 	struct fq *fq = &local->fq;
1435 	int ret;
1436 	int i;
1437 	bool supp_vht = false;
1438 	enum nl80211_band band;
1439 
1440 	if (!local->ops->wake_tx_queue)
1441 		return 0;
1442 
1443 	ret = fq_init(fq, 4096);
1444 	if (ret)
1445 		return ret;
1446 
1447 	/*
1448 	 * If the hardware doesn't support VHT, it is safe to limit the maximum
1449 	 * queue size. 4 Mbytes is 64 max-size aggregates in 802.11n.
1450 	 */
1451 	for (band = 0; band < NUM_NL80211_BANDS; band++) {
1452 		struct ieee80211_supported_band *sband;
1453 
1454 		sband = local->hw.wiphy->bands[band];
1455 		if (!sband)
1456 			continue;
1457 
1458 		supp_vht = supp_vht || sband->vht_cap.vht_supported;
1459 	}
1460 
1461 	if (!supp_vht)
1462 		fq->memory_limit = 4 << 20; /* 4 Mbytes */
1463 
1464 	codel_params_init(&local->cparams);
1465 	local->cparams.interval = MS2TIME(100);
1466 	local->cparams.target = MS2TIME(20);
1467 	local->cparams.ecn = true;
1468 
1469 	local->cvars = kcalloc(fq->flows_cnt, sizeof(local->cvars[0]),
1470 			       GFP_KERNEL);
1471 	if (!local->cvars) {
1472 		spin_lock_bh(&fq->lock);
1473 		fq_reset(fq, fq_skb_free_func);
1474 		spin_unlock_bh(&fq->lock);
1475 		return -ENOMEM;
1476 	}
1477 
1478 	for (i = 0; i < fq->flows_cnt; i++)
1479 		codel_vars_init(&local->cvars[i]);
1480 
1481 	return 0;
1482 }
1483 
1484 void ieee80211_txq_teardown_flows(struct ieee80211_local *local)
1485 {
1486 	struct fq *fq = &local->fq;
1487 
1488 	if (!local->ops->wake_tx_queue)
1489 		return;
1490 
1491 	kfree(local->cvars);
1492 	local->cvars = NULL;
1493 
1494 	spin_lock_bh(&fq->lock);
1495 	fq_reset(fq, fq_skb_free_func);
1496 	spin_unlock_bh(&fq->lock);
1497 }
1498 
1499 static bool ieee80211_queue_skb(struct ieee80211_local *local,
1500 				struct ieee80211_sub_if_data *sdata,
1501 				struct sta_info *sta,
1502 				struct sk_buff *skb)
1503 {
1504 	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
1505 	struct fq *fq = &local->fq;
1506 	struct ieee80211_vif *vif;
1507 	struct txq_info *txqi;
1508 	struct ieee80211_sta *pubsta;
1509 
1510 	if (!local->ops->wake_tx_queue ||
1511 	    sdata->vif.type == NL80211_IFTYPE_MONITOR)
1512 		return false;
1513 
1514 	if (sta && sta->uploaded)
1515 		pubsta = &sta->sta;
1516 	else
1517 		pubsta = NULL;
1518 
1519 	if (sdata->vif.type == NL80211_IFTYPE_AP_VLAN)
1520 		sdata = container_of(sdata->bss,
1521 				     struct ieee80211_sub_if_data, u.ap);
1522 
1523 	vif = &sdata->vif;
1524 	txqi = ieee80211_get_txq(local, vif, pubsta, skb);
1525 
1526 	if (!txqi)
1527 		return false;
1528 
1529 	info->control.vif = vif;
1530 
1531 	spin_lock_bh(&fq->lock);
1532 	ieee80211_txq_enqueue(local, txqi, skb);
1533 	spin_unlock_bh(&fq->lock);
1534 
1535 	drv_wake_tx_queue(local, txqi);
1536 
1537 	return true;
1538 }
1539 
1540 static bool ieee80211_tx_frags(struct ieee80211_local *local,
1541 			       struct ieee80211_vif *vif,
1542 			       struct ieee80211_sta *sta,
1543 			       struct sk_buff_head *skbs,
1544 			       bool txpending)
1545 {
1546 	struct ieee80211_tx_control control = {};
1547 	struct sk_buff *skb, *tmp;
1548 	unsigned long flags;
1549 
1550 	skb_queue_walk_safe(skbs, skb, tmp) {
1551 		struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
1552 		int q = info->hw_queue;
1553 
1554 #ifdef CONFIG_MAC80211_VERBOSE_DEBUG
1555 		if (WARN_ON_ONCE(q >= local->hw.queues)) {
1556 			__skb_unlink(skb, skbs);
1557 			ieee80211_free_txskb(&local->hw, skb);
1558 			continue;
1559 		}
1560 #endif
1561 
1562 		spin_lock_irqsave(&local->queue_stop_reason_lock, flags);
1563 		if (local->queue_stop_reasons[q] ||
1564 		    (!txpending && !skb_queue_empty(&local->pending[q]))) {
1565 			if (unlikely(info->flags &
1566 				     IEEE80211_TX_INTFL_OFFCHAN_TX_OK)) {
1567 				if (local->queue_stop_reasons[q] &
1568 				    ~BIT(IEEE80211_QUEUE_STOP_REASON_OFFCHANNEL)) {
1569 					/*
1570 					 * Drop off-channel frames if queues
1571 					 * are stopped for any reason other
1572 					 * than off-channel operation. Never
1573 					 * queue them.
1574 					 */
1575 					spin_unlock_irqrestore(
1576 						&local->queue_stop_reason_lock,
1577 						flags);
1578 					ieee80211_purge_tx_queue(&local->hw,
1579 								 skbs);
1580 					return true;
1581 				}
1582 			} else {
1583 
1584 				/*
1585 				 * Since queue is stopped, queue up frames for
1586 				 * later transmission from the tx-pending
1587 				 * tasklet when the queue is woken again.
1588 				 */
1589 				if (txpending)
1590 					skb_queue_splice_init(skbs,
1591 							      &local->pending[q]);
1592 				else
1593 					skb_queue_splice_tail_init(skbs,
1594 								   &local->pending[q]);
1595 
1596 				spin_unlock_irqrestore(&local->queue_stop_reason_lock,
1597 						       flags);
1598 				return false;
1599 			}
1600 		}
1601 		spin_unlock_irqrestore(&local->queue_stop_reason_lock, flags);
1602 
1603 		info->control.vif = vif;
1604 		control.sta = sta;
1605 
1606 		__skb_unlink(skb, skbs);
1607 		drv_tx(local, &control, skb);
1608 	}
1609 
1610 	return true;
1611 }
1612 
1613 /*
1614  * Returns false if the frame couldn't be transmitted but was queued instead.
1615  */
1616 static bool __ieee80211_tx(struct ieee80211_local *local,
1617 			   struct sk_buff_head *skbs, int led_len,
1618 			   struct sta_info *sta, bool txpending)
1619 {
1620 	struct ieee80211_tx_info *info;
1621 	struct ieee80211_sub_if_data *sdata;
1622 	struct ieee80211_vif *vif;
1623 	struct ieee80211_sta *pubsta;
1624 	struct sk_buff *skb;
1625 	bool result = true;
1626 	__le16 fc;
1627 
1628 	if (WARN_ON(skb_queue_empty(skbs)))
1629 		return true;
1630 
1631 	skb = skb_peek(skbs);
1632 	fc = ((struct ieee80211_hdr *)skb->data)->frame_control;
1633 	info = IEEE80211_SKB_CB(skb);
1634 	sdata = vif_to_sdata(info->control.vif);
1635 	if (sta && !sta->uploaded)
1636 		sta = NULL;
1637 
1638 	if (sta)
1639 		pubsta = &sta->sta;
1640 	else
1641 		pubsta = NULL;
1642 
1643 	switch (sdata->vif.type) {
1644 	case NL80211_IFTYPE_MONITOR:
1645 		if (sdata->u.mntr.flags & MONITOR_FLAG_ACTIVE) {
1646 			vif = &sdata->vif;
1647 			break;
1648 		}
1649 		sdata = rcu_dereference(local->monitor_sdata);
1650 		if (sdata) {
1651 			vif = &sdata->vif;
1652 			info->hw_queue =
1653 				vif->hw_queue[skb_get_queue_mapping(skb)];
1654 		} else if (ieee80211_hw_check(&local->hw, QUEUE_CONTROL)) {
1655 			ieee80211_purge_tx_queue(&local->hw, skbs);
1656 			return true;
1657 		} else
1658 			vif = NULL;
1659 		break;
1660 	case NL80211_IFTYPE_AP_VLAN:
1661 		sdata = container_of(sdata->bss,
1662 				     struct ieee80211_sub_if_data, u.ap);
1663 		/* fall through */
1664 	default:
1665 		vif = &sdata->vif;
1666 		break;
1667 	}
1668 
1669 	result = ieee80211_tx_frags(local, vif, pubsta, skbs,
1670 				    txpending);
1671 
1672 	ieee80211_tpt_led_trig_tx(local, fc, led_len);
1673 
1674 	WARN_ON_ONCE(!skb_queue_empty(skbs));
1675 
1676 	return result;
1677 }
1678 
1679 /*
1680  * Invoke TX handlers, return 0 on success and non-zero if the
1681  * frame was dropped or queued.
1682  *
1683  * The handlers are split into an early and late part. The latter is everything
1684  * that can be sensitive to reordering, and will be deferred to after packets
1685  * are dequeued from the intermediate queues (when they are enabled).
1686  */
1687 static int invoke_tx_handlers_early(struct ieee80211_tx_data *tx)
1688 {
1689 	ieee80211_tx_result res = TX_DROP;
1690 
1691 #define CALL_TXH(txh) \
1692 	do {				\
1693 		res = txh(tx);		\
1694 		if (res != TX_CONTINUE)	\
1695 			goto txh_done;	\
1696 	} while (0)
1697 
1698 	CALL_TXH(ieee80211_tx_h_dynamic_ps);
1699 	CALL_TXH(ieee80211_tx_h_check_assoc);
1700 	CALL_TXH(ieee80211_tx_h_ps_buf);
1701 	CALL_TXH(ieee80211_tx_h_check_control_port_protocol);
1702 	CALL_TXH(ieee80211_tx_h_select_key);
1703 	if (!ieee80211_hw_check(&tx->local->hw, HAS_RATE_CONTROL))
1704 		CALL_TXH(ieee80211_tx_h_rate_ctrl);
1705 
1706  txh_done:
1707 	if (unlikely(res == TX_DROP)) {
1708 		I802_DEBUG_INC(tx->local->tx_handlers_drop);
1709 		if (tx->skb)
1710 			ieee80211_free_txskb(&tx->local->hw, tx->skb);
1711 		else
1712 			ieee80211_purge_tx_queue(&tx->local->hw, &tx->skbs);
1713 		return -1;
1714 	} else if (unlikely(res == TX_QUEUED)) {
1715 		I802_DEBUG_INC(tx->local->tx_handlers_queued);
1716 		return -1;
1717 	}
1718 
1719 	return 0;
1720 }
1721 
1722 /*
1723  * Late handlers can be called while the sta lock is held. Handlers that can
1724  * cause packets to be generated will cause deadlock!
1725  */
1726 static int invoke_tx_handlers_late(struct ieee80211_tx_data *tx)
1727 {
1728 	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(tx->skb);
1729 	ieee80211_tx_result res = TX_CONTINUE;
1730 
1731 	if (unlikely(info->flags & IEEE80211_TX_INTFL_RETRANSMISSION)) {
1732 		__skb_queue_tail(&tx->skbs, tx->skb);
1733 		tx->skb = NULL;
1734 		goto txh_done;
1735 	}
1736 
1737 	CALL_TXH(ieee80211_tx_h_michael_mic_add);
1738 	CALL_TXH(ieee80211_tx_h_sequence);
1739 	CALL_TXH(ieee80211_tx_h_fragment);
1740 	/* handlers after fragment must be aware of tx info fragmentation! */
1741 	CALL_TXH(ieee80211_tx_h_stats);
1742 	CALL_TXH(ieee80211_tx_h_encrypt);
1743 	if (!ieee80211_hw_check(&tx->local->hw, HAS_RATE_CONTROL))
1744 		CALL_TXH(ieee80211_tx_h_calculate_duration);
1745 #undef CALL_TXH
1746 
1747  txh_done:
1748 	if (unlikely(res == TX_DROP)) {
1749 		I802_DEBUG_INC(tx->local->tx_handlers_drop);
1750 		if (tx->skb)
1751 			ieee80211_free_txskb(&tx->local->hw, tx->skb);
1752 		else
1753 			ieee80211_purge_tx_queue(&tx->local->hw, &tx->skbs);
1754 		return -1;
1755 	} else if (unlikely(res == TX_QUEUED)) {
1756 		I802_DEBUG_INC(tx->local->tx_handlers_queued);
1757 		return -1;
1758 	}
1759 
1760 	return 0;
1761 }
1762 
1763 static int invoke_tx_handlers(struct ieee80211_tx_data *tx)
1764 {
1765 	int r = invoke_tx_handlers_early(tx);
1766 
1767 	if (r)
1768 		return r;
1769 	return invoke_tx_handlers_late(tx);
1770 }
1771 
1772 bool ieee80211_tx_prepare_skb(struct ieee80211_hw *hw,
1773 			      struct ieee80211_vif *vif, struct sk_buff *skb,
1774 			      int band, struct ieee80211_sta **sta)
1775 {
1776 	struct ieee80211_sub_if_data *sdata = vif_to_sdata(vif);
1777 	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
1778 	struct ieee80211_tx_data tx;
1779 	struct sk_buff *skb2;
1780 
1781 	if (ieee80211_tx_prepare(sdata, &tx, NULL, skb) == TX_DROP)
1782 		return false;
1783 
1784 	info->band = band;
1785 	info->control.vif = vif;
1786 	info->hw_queue = vif->hw_queue[skb_get_queue_mapping(skb)];
1787 
1788 	if (invoke_tx_handlers(&tx))
1789 		return false;
1790 
1791 	if (sta) {
1792 		if (tx.sta)
1793 			*sta = &tx.sta->sta;
1794 		else
1795 			*sta = NULL;
1796 	}
1797 
1798 	/* this function isn't suitable for fragmented data frames */
1799 	skb2 = __skb_dequeue(&tx.skbs);
1800 	if (WARN_ON(skb2 != skb || !skb_queue_empty(&tx.skbs))) {
1801 		ieee80211_free_txskb(hw, skb2);
1802 		ieee80211_purge_tx_queue(hw, &tx.skbs);
1803 		return false;
1804 	}
1805 
1806 	return true;
1807 }
1808 EXPORT_SYMBOL(ieee80211_tx_prepare_skb);
1809 
1810 /*
1811  * Returns false if the frame couldn't be transmitted but was queued instead.
1812  */
1813 static bool ieee80211_tx(struct ieee80211_sub_if_data *sdata,
1814 			 struct sta_info *sta, struct sk_buff *skb,
1815 			 bool txpending)
1816 {
1817 	struct ieee80211_local *local = sdata->local;
1818 	struct ieee80211_tx_data tx;
1819 	ieee80211_tx_result res_prepare;
1820 	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
1821 	bool result = true;
1822 	int led_len;
1823 
1824 	if (unlikely(skb->len < 10)) {
1825 		dev_kfree_skb(skb);
1826 		return true;
1827 	}
1828 
1829 	/* initialises tx */
1830 	led_len = skb->len;
1831 	res_prepare = ieee80211_tx_prepare(sdata, &tx, sta, skb);
1832 
1833 	if (unlikely(res_prepare == TX_DROP)) {
1834 		ieee80211_free_txskb(&local->hw, skb);
1835 		return true;
1836 	} else if (unlikely(res_prepare == TX_QUEUED)) {
1837 		return true;
1838 	}
1839 
1840 	/* set up hw_queue value early */
1841 	if (!(info->flags & IEEE80211_TX_CTL_TX_OFFCHAN) ||
1842 	    !ieee80211_hw_check(&local->hw, QUEUE_CONTROL))
1843 		info->hw_queue =
1844 			sdata->vif.hw_queue[skb_get_queue_mapping(skb)];
1845 
1846 	if (invoke_tx_handlers_early(&tx))
1847 		return false;
1848 
1849 	if (ieee80211_queue_skb(local, sdata, tx.sta, tx.skb))
1850 		return true;
1851 
1852 	if (!invoke_tx_handlers_late(&tx))
1853 		result = __ieee80211_tx(local, &tx.skbs, led_len,
1854 					tx.sta, txpending);
1855 
1856 	return result;
1857 }
1858 
1859 /* device xmit handlers */
1860 
1861 static int ieee80211_skb_resize(struct ieee80211_sub_if_data *sdata,
1862 				struct sk_buff *skb,
1863 				int head_need, bool may_encrypt)
1864 {
1865 	struct ieee80211_local *local = sdata->local;
1866 	int tail_need = 0;
1867 
1868 	if (may_encrypt && sdata->crypto_tx_tailroom_needed_cnt) {
1869 		tail_need = IEEE80211_ENCRYPT_TAILROOM;
1870 		tail_need -= skb_tailroom(skb);
1871 		tail_need = max_t(int, tail_need, 0);
1872 	}
1873 
1874 	if (skb_cloned(skb) &&
1875 	    (!ieee80211_hw_check(&local->hw, SUPPORTS_CLONED_SKBS) ||
1876 	     !skb_clone_writable(skb, ETH_HLEN) ||
1877 	     (may_encrypt && sdata->crypto_tx_tailroom_needed_cnt)))
1878 		I802_DEBUG_INC(local->tx_expand_skb_head_cloned);
1879 	else if (head_need || tail_need)
1880 		I802_DEBUG_INC(local->tx_expand_skb_head);
1881 	else
1882 		return 0;
1883 
1884 	if (pskb_expand_head(skb, head_need, tail_need, GFP_ATOMIC)) {
1885 		wiphy_debug(local->hw.wiphy,
1886 			    "failed to reallocate TX buffer\n");
1887 		return -ENOMEM;
1888 	}
1889 
1890 	return 0;
1891 }
1892 
1893 void ieee80211_xmit(struct ieee80211_sub_if_data *sdata,
1894 		    struct sta_info *sta, struct sk_buff *skb)
1895 {
1896 	struct ieee80211_local *local = sdata->local;
1897 	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
1898 	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
1899 	int headroom;
1900 	bool may_encrypt;
1901 
1902 	may_encrypt = !(info->flags & IEEE80211_TX_INTFL_DONT_ENCRYPT);
1903 
1904 	headroom = local->tx_headroom;
1905 	if (may_encrypt)
1906 		headroom += sdata->encrypt_headroom;
1907 	headroom -= skb_headroom(skb);
1908 	headroom = max_t(int, 0, headroom);
1909 
1910 	if (ieee80211_skb_resize(sdata, skb, headroom, may_encrypt)) {
1911 		ieee80211_free_txskb(&local->hw, skb);
1912 		return;
1913 	}
1914 
1915 	hdr = (struct ieee80211_hdr *) skb->data;
1916 	info->control.vif = &sdata->vif;
1917 
1918 	if (ieee80211_vif_is_mesh(&sdata->vif)) {
1919 		if (ieee80211_is_data(hdr->frame_control) &&
1920 		    is_unicast_ether_addr(hdr->addr1)) {
1921 			if (mesh_nexthop_resolve(sdata, skb))
1922 				return; /* skb queued: don't free */
1923 		} else {
1924 			ieee80211_mps_set_frame_flags(sdata, NULL, hdr);
1925 		}
1926 	}
1927 
1928 	ieee80211_set_qos_hdr(sdata, skb);
1929 	ieee80211_tx(sdata, sta, skb, false);
1930 }
1931 
1932 static bool ieee80211_parse_tx_radiotap(struct ieee80211_local *local,
1933 					struct sk_buff *skb)
1934 {
1935 	struct ieee80211_radiotap_iterator iterator;
1936 	struct ieee80211_radiotap_header *rthdr =
1937 		(struct ieee80211_radiotap_header *) skb->data;
1938 	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
1939 	struct ieee80211_supported_band *sband =
1940 		local->hw.wiphy->bands[info->band];
1941 	int ret = ieee80211_radiotap_iterator_init(&iterator, rthdr, skb->len,
1942 						   NULL);
1943 	u16 txflags;
1944 	u16 rate = 0;
1945 	bool rate_found = false;
1946 	u8 rate_retries = 0;
1947 	u16 rate_flags = 0;
1948 	u8 mcs_known, mcs_flags, mcs_bw;
1949 	u16 vht_known;
1950 	u8 vht_mcs = 0, vht_nss = 0;
1951 	int i;
1952 
1953 	info->flags |= IEEE80211_TX_INTFL_DONT_ENCRYPT |
1954 		       IEEE80211_TX_CTL_DONTFRAG;
1955 
1956 	/*
1957 	 * for every radiotap entry that is present
1958 	 * (ieee80211_radiotap_iterator_next returns -ENOENT when no more
1959 	 * entries present, or -EINVAL on error)
1960 	 */
1961 
1962 	while (!ret) {
1963 		ret = ieee80211_radiotap_iterator_next(&iterator);
1964 
1965 		if (ret)
1966 			continue;
1967 
1968 		/* see if this argument is something we can use */
1969 		switch (iterator.this_arg_index) {
1970 		/*
1971 		 * You must take care when dereferencing iterator.this_arg
1972 		 * for multibyte types... the pointer is not aligned.  Use
1973 		 * get_unaligned((type *)iterator.this_arg) to dereference
1974 		 * iterator.this_arg for type "type" safely on all arches.
1975 		*/
1976 		case IEEE80211_RADIOTAP_FLAGS:
1977 			if (*iterator.this_arg & IEEE80211_RADIOTAP_F_FCS) {
1978 				/*
1979 				 * this indicates that the skb we have been
1980 				 * handed has the 32-bit FCS CRC at the end...
1981 				 * we should react to that by snipping it off
1982 				 * because it will be recomputed and added
1983 				 * on transmission
1984 				 */
1985 				if (skb->len < (iterator._max_length + FCS_LEN))
1986 					return false;
1987 
1988 				skb_trim(skb, skb->len - FCS_LEN);
1989 			}
1990 			if (*iterator.this_arg & IEEE80211_RADIOTAP_F_WEP)
1991 				info->flags &= ~IEEE80211_TX_INTFL_DONT_ENCRYPT;
1992 			if (*iterator.this_arg & IEEE80211_RADIOTAP_F_FRAG)
1993 				info->flags &= ~IEEE80211_TX_CTL_DONTFRAG;
1994 			break;
1995 
1996 		case IEEE80211_RADIOTAP_TX_FLAGS:
1997 			txflags = get_unaligned_le16(iterator.this_arg);
1998 			if (txflags & IEEE80211_RADIOTAP_F_TX_NOACK)
1999 				info->flags |= IEEE80211_TX_CTL_NO_ACK;
2000 			break;
2001 
2002 		case IEEE80211_RADIOTAP_RATE:
2003 			rate = *iterator.this_arg;
2004 			rate_flags = 0;
2005 			rate_found = true;
2006 			break;
2007 
2008 		case IEEE80211_RADIOTAP_DATA_RETRIES:
2009 			rate_retries = *iterator.this_arg;
2010 			break;
2011 
2012 		case IEEE80211_RADIOTAP_MCS:
2013 			mcs_known = iterator.this_arg[0];
2014 			mcs_flags = iterator.this_arg[1];
2015 			if (!(mcs_known & IEEE80211_RADIOTAP_MCS_HAVE_MCS))
2016 				break;
2017 
2018 			rate_found = true;
2019 			rate = iterator.this_arg[2];
2020 			rate_flags = IEEE80211_TX_RC_MCS;
2021 
2022 			if (mcs_known & IEEE80211_RADIOTAP_MCS_HAVE_GI &&
2023 			    mcs_flags & IEEE80211_RADIOTAP_MCS_SGI)
2024 				rate_flags |= IEEE80211_TX_RC_SHORT_GI;
2025 
2026 			mcs_bw = mcs_flags & IEEE80211_RADIOTAP_MCS_BW_MASK;
2027 			if (mcs_known & IEEE80211_RADIOTAP_MCS_HAVE_BW &&
2028 			    mcs_bw == IEEE80211_RADIOTAP_MCS_BW_40)
2029 				rate_flags |= IEEE80211_TX_RC_40_MHZ_WIDTH;
2030 			break;
2031 
2032 		case IEEE80211_RADIOTAP_VHT:
2033 			vht_known = get_unaligned_le16(iterator.this_arg);
2034 			rate_found = true;
2035 
2036 			rate_flags = IEEE80211_TX_RC_VHT_MCS;
2037 			if ((vht_known & IEEE80211_RADIOTAP_VHT_KNOWN_GI) &&
2038 			    (iterator.this_arg[2] &
2039 			     IEEE80211_RADIOTAP_VHT_FLAG_SGI))
2040 				rate_flags |= IEEE80211_TX_RC_SHORT_GI;
2041 			if (vht_known &
2042 			    IEEE80211_RADIOTAP_VHT_KNOWN_BANDWIDTH) {
2043 				if (iterator.this_arg[3] == 1)
2044 					rate_flags |=
2045 						IEEE80211_TX_RC_40_MHZ_WIDTH;
2046 				else if (iterator.this_arg[3] == 4)
2047 					rate_flags |=
2048 						IEEE80211_TX_RC_80_MHZ_WIDTH;
2049 				else if (iterator.this_arg[3] == 11)
2050 					rate_flags |=
2051 						IEEE80211_TX_RC_160_MHZ_WIDTH;
2052 			}
2053 
2054 			vht_mcs = iterator.this_arg[4] >> 4;
2055 			vht_nss = iterator.this_arg[4] & 0xF;
2056 			break;
2057 
2058 		/*
2059 		 * Please update the file
2060 		 * Documentation/networking/mac80211-injection.txt
2061 		 * when parsing new fields here.
2062 		 */
2063 
2064 		default:
2065 			break;
2066 		}
2067 	}
2068 
2069 	if (ret != -ENOENT) /* ie, if we didn't simply run out of fields */
2070 		return false;
2071 
2072 	if (rate_found) {
2073 		info->control.flags |= IEEE80211_TX_CTRL_RATE_INJECT;
2074 
2075 		for (i = 0; i < IEEE80211_TX_MAX_RATES; i++) {
2076 			info->control.rates[i].idx = -1;
2077 			info->control.rates[i].flags = 0;
2078 			info->control.rates[i].count = 0;
2079 		}
2080 
2081 		if (rate_flags & IEEE80211_TX_RC_MCS) {
2082 			info->control.rates[0].idx = rate;
2083 		} else if (rate_flags & IEEE80211_TX_RC_VHT_MCS) {
2084 			ieee80211_rate_set_vht(info->control.rates, vht_mcs,
2085 					       vht_nss);
2086 		} else {
2087 			for (i = 0; i < sband->n_bitrates; i++) {
2088 				if (rate * 5 != sband->bitrates[i].bitrate)
2089 					continue;
2090 
2091 				info->control.rates[0].idx = i;
2092 				break;
2093 			}
2094 		}
2095 
2096 		if (info->control.rates[0].idx < 0)
2097 			info->control.flags &= ~IEEE80211_TX_CTRL_RATE_INJECT;
2098 
2099 		info->control.rates[0].flags = rate_flags;
2100 		info->control.rates[0].count = min_t(u8, rate_retries + 1,
2101 						     local->hw.max_rate_tries);
2102 	}
2103 
2104 	/*
2105 	 * remove the radiotap header
2106 	 * iterator->_max_length was sanity-checked against
2107 	 * skb->len by iterator init
2108 	 */
2109 	skb_pull(skb, iterator._max_length);
2110 
2111 	return true;
2112 }
2113 
2114 netdev_tx_t ieee80211_monitor_start_xmit(struct sk_buff *skb,
2115 					 struct net_device *dev)
2116 {
2117 	struct ieee80211_local *local = wdev_priv(dev->ieee80211_ptr);
2118 	struct ieee80211_chanctx_conf *chanctx_conf;
2119 	struct ieee80211_radiotap_header *prthdr =
2120 		(struct ieee80211_radiotap_header *)skb->data;
2121 	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
2122 	struct ieee80211_hdr *hdr;
2123 	struct ieee80211_sub_if_data *tmp_sdata, *sdata;
2124 	struct cfg80211_chan_def *chandef;
2125 	u16 len_rthdr;
2126 	int hdrlen;
2127 
2128 	/* check for not even having the fixed radiotap header part */
2129 	if (unlikely(skb->len < sizeof(struct ieee80211_radiotap_header)))
2130 		goto fail; /* too short to be possibly valid */
2131 
2132 	/* is it a header version we can trust to find length from? */
2133 	if (unlikely(prthdr->it_version))
2134 		goto fail; /* only version 0 is supported */
2135 
2136 	/* then there must be a radiotap header with a length we can use */
2137 	len_rthdr = ieee80211_get_radiotap_len(skb->data);
2138 
2139 	/* does the skb contain enough to deliver on the alleged length? */
2140 	if (unlikely(skb->len < len_rthdr))
2141 		goto fail; /* skb too short for claimed rt header extent */
2142 
2143 	/*
2144 	 * fix up the pointers accounting for the radiotap
2145 	 * header still being in there.  We are being given
2146 	 * a precooked IEEE80211 header so no need for
2147 	 * normal processing
2148 	 */
2149 	skb_set_mac_header(skb, len_rthdr);
2150 	/*
2151 	 * these are just fixed to the end of the rt area since we
2152 	 * don't have any better information and at this point, nobody cares
2153 	 */
2154 	skb_set_network_header(skb, len_rthdr);
2155 	skb_set_transport_header(skb, len_rthdr);
2156 
2157 	if (skb->len < len_rthdr + 2)
2158 		goto fail;
2159 
2160 	hdr = (struct ieee80211_hdr *)(skb->data + len_rthdr);
2161 	hdrlen = ieee80211_hdrlen(hdr->frame_control);
2162 
2163 	if (skb->len < len_rthdr + hdrlen)
2164 		goto fail;
2165 
2166 	/*
2167 	 * Initialize skb->protocol if the injected frame is a data frame
2168 	 * carrying a rfc1042 header
2169 	 */
2170 	if (ieee80211_is_data(hdr->frame_control) &&
2171 	    skb->len >= len_rthdr + hdrlen + sizeof(rfc1042_header) + 2) {
2172 		u8 *payload = (u8 *)hdr + hdrlen;
2173 
2174 		if (ether_addr_equal(payload, rfc1042_header))
2175 			skb->protocol = cpu_to_be16((payload[6] << 8) |
2176 						    payload[7]);
2177 	}
2178 
2179 	memset(info, 0, sizeof(*info));
2180 
2181 	info->flags = IEEE80211_TX_CTL_REQ_TX_STATUS |
2182 		      IEEE80211_TX_CTL_INJECTED;
2183 
2184 	rcu_read_lock();
2185 
2186 	/*
2187 	 * We process outgoing injected frames that have a local address
2188 	 * we handle as though they are non-injected frames.
2189 	 * This code here isn't entirely correct, the local MAC address
2190 	 * isn't always enough to find the interface to use; for proper
2191 	 * VLAN/WDS support we will need a different mechanism (which
2192 	 * likely isn't going to be monitor interfaces).
2193 	 */
2194 	sdata = IEEE80211_DEV_TO_SUB_IF(dev);
2195 
2196 	list_for_each_entry_rcu(tmp_sdata, &local->interfaces, list) {
2197 		if (!ieee80211_sdata_running(tmp_sdata))
2198 			continue;
2199 		if (tmp_sdata->vif.type == NL80211_IFTYPE_MONITOR ||
2200 		    tmp_sdata->vif.type == NL80211_IFTYPE_AP_VLAN ||
2201 		    tmp_sdata->vif.type == NL80211_IFTYPE_WDS)
2202 			continue;
2203 		if (ether_addr_equal(tmp_sdata->vif.addr, hdr->addr2)) {
2204 			sdata = tmp_sdata;
2205 			break;
2206 		}
2207 	}
2208 
2209 	chanctx_conf = rcu_dereference(sdata->vif.chanctx_conf);
2210 	if (!chanctx_conf) {
2211 		tmp_sdata = rcu_dereference(local->monitor_sdata);
2212 		if (tmp_sdata)
2213 			chanctx_conf =
2214 				rcu_dereference(tmp_sdata->vif.chanctx_conf);
2215 	}
2216 
2217 	if (chanctx_conf)
2218 		chandef = &chanctx_conf->def;
2219 	else if (!local->use_chanctx)
2220 		chandef = &local->_oper_chandef;
2221 	else
2222 		goto fail_rcu;
2223 
2224 	/*
2225 	 * Frame injection is not allowed if beaconing is not allowed
2226 	 * or if we need radar detection. Beaconing is usually not allowed when
2227 	 * the mode or operation (Adhoc, AP, Mesh) does not support DFS.
2228 	 * Passive scan is also used in world regulatory domains where
2229 	 * your country is not known and as such it should be treated as
2230 	 * NO TX unless the channel is explicitly allowed in which case
2231 	 * your current regulatory domain would not have the passive scan
2232 	 * flag.
2233 	 *
2234 	 * Since AP mode uses monitor interfaces to inject/TX management
2235 	 * frames we can make AP mode the exception to this rule once it
2236 	 * supports radar detection as its implementation can deal with
2237 	 * radar detection by itself. We can do that later by adding a
2238 	 * monitor flag interfaces used for AP support.
2239 	 */
2240 	if (!cfg80211_reg_can_beacon(local->hw.wiphy, chandef,
2241 				     sdata->vif.type))
2242 		goto fail_rcu;
2243 
2244 	info->band = chandef->chan->band;
2245 
2246 	/* process and remove the injection radiotap header */
2247 	if (!ieee80211_parse_tx_radiotap(local, skb))
2248 		goto fail_rcu;
2249 
2250 	ieee80211_xmit(sdata, NULL, skb);
2251 	rcu_read_unlock();
2252 
2253 	return NETDEV_TX_OK;
2254 
2255 fail_rcu:
2256 	rcu_read_unlock();
2257 fail:
2258 	dev_kfree_skb(skb);
2259 	return NETDEV_TX_OK; /* meaning, we dealt with the skb */
2260 }
2261 
2262 static inline bool ieee80211_is_tdls_setup(struct sk_buff *skb)
2263 {
2264 	u16 ethertype = (skb->data[12] << 8) | skb->data[13];
2265 
2266 	return ethertype == ETH_P_TDLS &&
2267 	       skb->len > 14 &&
2268 	       skb->data[14] == WLAN_TDLS_SNAP_RFTYPE;
2269 }
2270 
2271 static int ieee80211_lookup_ra_sta(struct ieee80211_sub_if_data *sdata,
2272 				   struct sk_buff *skb,
2273 				   struct sta_info **sta_out)
2274 {
2275 	struct sta_info *sta;
2276 
2277 	switch (sdata->vif.type) {
2278 	case NL80211_IFTYPE_AP_VLAN:
2279 		sta = rcu_dereference(sdata->u.vlan.sta);
2280 		if (sta) {
2281 			*sta_out = sta;
2282 			return 0;
2283 		} else if (sdata->wdev.use_4addr) {
2284 			return -ENOLINK;
2285 		}
2286 		/* fall through */
2287 	case NL80211_IFTYPE_AP:
2288 	case NL80211_IFTYPE_OCB:
2289 	case NL80211_IFTYPE_ADHOC:
2290 		if (is_multicast_ether_addr(skb->data)) {
2291 			*sta_out = ERR_PTR(-ENOENT);
2292 			return 0;
2293 		}
2294 		sta = sta_info_get_bss(sdata, skb->data);
2295 		break;
2296 	case NL80211_IFTYPE_WDS:
2297 		sta = sta_info_get(sdata, sdata->u.wds.remote_addr);
2298 		break;
2299 #ifdef CONFIG_MAC80211_MESH
2300 	case NL80211_IFTYPE_MESH_POINT:
2301 		/* determined much later */
2302 		*sta_out = NULL;
2303 		return 0;
2304 #endif
2305 	case NL80211_IFTYPE_STATION:
2306 		if (sdata->wdev.wiphy->flags & WIPHY_FLAG_SUPPORTS_TDLS) {
2307 			sta = sta_info_get(sdata, skb->data);
2308 			if (sta && test_sta_flag(sta, WLAN_STA_TDLS_PEER)) {
2309 				if (test_sta_flag(sta,
2310 						  WLAN_STA_TDLS_PEER_AUTH)) {
2311 					*sta_out = sta;
2312 					return 0;
2313 				}
2314 
2315 				/*
2316 				 * TDLS link during setup - throw out frames to
2317 				 * peer. Allow TDLS-setup frames to unauthorized
2318 				 * peers for the special case of a link teardown
2319 				 * after a TDLS sta is removed due to being
2320 				 * unreachable.
2321 				 */
2322 				if (!ieee80211_is_tdls_setup(skb))
2323 					return -EINVAL;
2324 			}
2325 
2326 		}
2327 
2328 		sta = sta_info_get(sdata, sdata->u.mgd.bssid);
2329 		if (!sta)
2330 			return -ENOLINK;
2331 		break;
2332 	default:
2333 		return -EINVAL;
2334 	}
2335 
2336 	*sta_out = sta ?: ERR_PTR(-ENOENT);
2337 	return 0;
2338 }
2339 
2340 /**
2341  * ieee80211_build_hdr - build 802.11 header in the given frame
2342  * @sdata: virtual interface to build the header for
2343  * @skb: the skb to build the header in
2344  * @info_flags: skb flags to set
2345  *
2346  * This function takes the skb with 802.3 header and reformats the header to
2347  * the appropriate IEEE 802.11 header based on which interface the packet is
2348  * being transmitted on.
2349  *
2350  * Note that this function also takes care of the TX status request and
2351  * potential unsharing of the SKB - this needs to be interleaved with the
2352  * header building.
2353  *
2354  * The function requires the read-side RCU lock held
2355  *
2356  * Returns: the (possibly reallocated) skb or an ERR_PTR() code
2357  */
2358 static struct sk_buff *ieee80211_build_hdr(struct ieee80211_sub_if_data *sdata,
2359 					   struct sk_buff *skb, u32 info_flags,
2360 					   struct sta_info *sta)
2361 {
2362 	struct ieee80211_local *local = sdata->local;
2363 	struct ieee80211_tx_info *info;
2364 	int head_need;
2365 	u16 ethertype, hdrlen,  meshhdrlen = 0;
2366 	__le16 fc;
2367 	struct ieee80211_hdr hdr;
2368 	struct ieee80211s_hdr mesh_hdr __maybe_unused;
2369 	struct mesh_path __maybe_unused *mppath = NULL, *mpath = NULL;
2370 	const u8 *encaps_data;
2371 	int encaps_len, skip_header_bytes;
2372 	bool wme_sta = false, authorized = false;
2373 	bool tdls_peer;
2374 	bool multicast;
2375 	u16 info_id = 0;
2376 	struct ieee80211_chanctx_conf *chanctx_conf;
2377 	struct ieee80211_sub_if_data *ap_sdata;
2378 	enum nl80211_band band;
2379 	int ret;
2380 
2381 	if (IS_ERR(sta))
2382 		sta = NULL;
2383 
2384 	/* convert Ethernet header to proper 802.11 header (based on
2385 	 * operation mode) */
2386 	ethertype = (skb->data[12] << 8) | skb->data[13];
2387 	fc = cpu_to_le16(IEEE80211_FTYPE_DATA | IEEE80211_STYPE_DATA);
2388 
2389 	switch (sdata->vif.type) {
2390 	case NL80211_IFTYPE_AP_VLAN:
2391 		if (sdata->wdev.use_4addr) {
2392 			fc |= cpu_to_le16(IEEE80211_FCTL_FROMDS | IEEE80211_FCTL_TODS);
2393 			/* RA TA DA SA */
2394 			memcpy(hdr.addr1, sta->sta.addr, ETH_ALEN);
2395 			memcpy(hdr.addr2, sdata->vif.addr, ETH_ALEN);
2396 			memcpy(hdr.addr3, skb->data, ETH_ALEN);
2397 			memcpy(hdr.addr4, skb->data + ETH_ALEN, ETH_ALEN);
2398 			hdrlen = 30;
2399 			authorized = test_sta_flag(sta, WLAN_STA_AUTHORIZED);
2400 			wme_sta = sta->sta.wme;
2401 		}
2402 		ap_sdata = container_of(sdata->bss, struct ieee80211_sub_if_data,
2403 					u.ap);
2404 		chanctx_conf = rcu_dereference(ap_sdata->vif.chanctx_conf);
2405 		if (!chanctx_conf) {
2406 			ret = -ENOTCONN;
2407 			goto free;
2408 		}
2409 		band = chanctx_conf->def.chan->band;
2410 		if (sdata->wdev.use_4addr)
2411 			break;
2412 		/* fall through */
2413 	case NL80211_IFTYPE_AP:
2414 		if (sdata->vif.type == NL80211_IFTYPE_AP)
2415 			chanctx_conf = rcu_dereference(sdata->vif.chanctx_conf);
2416 		if (!chanctx_conf) {
2417 			ret = -ENOTCONN;
2418 			goto free;
2419 		}
2420 		fc |= cpu_to_le16(IEEE80211_FCTL_FROMDS);
2421 		/* DA BSSID SA */
2422 		memcpy(hdr.addr1, skb->data, ETH_ALEN);
2423 		memcpy(hdr.addr2, sdata->vif.addr, ETH_ALEN);
2424 		memcpy(hdr.addr3, skb->data + ETH_ALEN, ETH_ALEN);
2425 		hdrlen = 24;
2426 		band = chanctx_conf->def.chan->band;
2427 		break;
2428 	case NL80211_IFTYPE_WDS:
2429 		fc |= cpu_to_le16(IEEE80211_FCTL_FROMDS | IEEE80211_FCTL_TODS);
2430 		/* RA TA DA SA */
2431 		memcpy(hdr.addr1, sdata->u.wds.remote_addr, ETH_ALEN);
2432 		memcpy(hdr.addr2, sdata->vif.addr, ETH_ALEN);
2433 		memcpy(hdr.addr3, skb->data, ETH_ALEN);
2434 		memcpy(hdr.addr4, skb->data + ETH_ALEN, ETH_ALEN);
2435 		hdrlen = 30;
2436 		/*
2437 		 * This is the exception! WDS style interfaces are prohibited
2438 		 * when channel contexts are in used so this must be valid
2439 		 */
2440 		band = local->hw.conf.chandef.chan->band;
2441 		break;
2442 #ifdef CONFIG_MAC80211_MESH
2443 	case NL80211_IFTYPE_MESH_POINT:
2444 		if (!is_multicast_ether_addr(skb->data)) {
2445 			struct sta_info *next_hop;
2446 			bool mpp_lookup = true;
2447 
2448 			mpath = mesh_path_lookup(sdata, skb->data);
2449 			if (mpath) {
2450 				mpp_lookup = false;
2451 				next_hop = rcu_dereference(mpath->next_hop);
2452 				if (!next_hop ||
2453 				    !(mpath->flags & (MESH_PATH_ACTIVE |
2454 						      MESH_PATH_RESOLVING)))
2455 					mpp_lookup = true;
2456 			}
2457 
2458 			if (mpp_lookup) {
2459 				mppath = mpp_path_lookup(sdata, skb->data);
2460 				if (mppath)
2461 					mppath->exp_time = jiffies;
2462 			}
2463 
2464 			if (mppath && mpath)
2465 				mesh_path_del(sdata, mpath->dst);
2466 		}
2467 
2468 		/*
2469 		 * Use address extension if it is a packet from
2470 		 * another interface or if we know the destination
2471 		 * is being proxied by a portal (i.e. portal address
2472 		 * differs from proxied address)
2473 		 */
2474 		if (ether_addr_equal(sdata->vif.addr, skb->data + ETH_ALEN) &&
2475 		    !(mppath && !ether_addr_equal(mppath->mpp, skb->data))) {
2476 			hdrlen = ieee80211_fill_mesh_addresses(&hdr, &fc,
2477 					skb->data, skb->data + ETH_ALEN);
2478 			meshhdrlen = ieee80211_new_mesh_header(sdata, &mesh_hdr,
2479 							       NULL, NULL);
2480 		} else {
2481 			/* DS -> MBSS (802.11-2012 13.11.3.3).
2482 			 * For unicast with unknown forwarding information,
2483 			 * destination might be in the MBSS or if that fails
2484 			 * forwarded to another mesh gate. In either case
2485 			 * resolution will be handled in ieee80211_xmit(), so
2486 			 * leave the original DA. This also works for mcast */
2487 			const u8 *mesh_da = skb->data;
2488 
2489 			if (mppath)
2490 				mesh_da = mppath->mpp;
2491 			else if (mpath)
2492 				mesh_da = mpath->dst;
2493 
2494 			hdrlen = ieee80211_fill_mesh_addresses(&hdr, &fc,
2495 					mesh_da, sdata->vif.addr);
2496 			if (is_multicast_ether_addr(mesh_da))
2497 				/* DA TA mSA AE:SA */
2498 				meshhdrlen = ieee80211_new_mesh_header(
2499 						sdata, &mesh_hdr,
2500 						skb->data + ETH_ALEN, NULL);
2501 			else
2502 				/* RA TA mDA mSA AE:DA SA */
2503 				meshhdrlen = ieee80211_new_mesh_header(
2504 						sdata, &mesh_hdr, skb->data,
2505 						skb->data + ETH_ALEN);
2506 
2507 		}
2508 		chanctx_conf = rcu_dereference(sdata->vif.chanctx_conf);
2509 		if (!chanctx_conf) {
2510 			ret = -ENOTCONN;
2511 			goto free;
2512 		}
2513 		band = chanctx_conf->def.chan->band;
2514 		break;
2515 #endif
2516 	case NL80211_IFTYPE_STATION:
2517 		/* we already did checks when looking up the RA STA */
2518 		tdls_peer = test_sta_flag(sta, WLAN_STA_TDLS_PEER);
2519 
2520 		if (tdls_peer) {
2521 			/* DA SA BSSID */
2522 			memcpy(hdr.addr1, skb->data, ETH_ALEN);
2523 			memcpy(hdr.addr2, skb->data + ETH_ALEN, ETH_ALEN);
2524 			memcpy(hdr.addr3, sdata->u.mgd.bssid, ETH_ALEN);
2525 			hdrlen = 24;
2526 		}  else if (sdata->u.mgd.use_4addr &&
2527 			    cpu_to_be16(ethertype) != sdata->control_port_protocol) {
2528 			fc |= cpu_to_le16(IEEE80211_FCTL_FROMDS |
2529 					  IEEE80211_FCTL_TODS);
2530 			/* RA TA DA SA */
2531 			memcpy(hdr.addr1, sdata->u.mgd.bssid, ETH_ALEN);
2532 			memcpy(hdr.addr2, sdata->vif.addr, ETH_ALEN);
2533 			memcpy(hdr.addr3, skb->data, ETH_ALEN);
2534 			memcpy(hdr.addr4, skb->data + ETH_ALEN, ETH_ALEN);
2535 			hdrlen = 30;
2536 		} else {
2537 			fc |= cpu_to_le16(IEEE80211_FCTL_TODS);
2538 			/* BSSID SA DA */
2539 			memcpy(hdr.addr1, sdata->u.mgd.bssid, ETH_ALEN);
2540 			memcpy(hdr.addr2, skb->data + ETH_ALEN, ETH_ALEN);
2541 			memcpy(hdr.addr3, skb->data, ETH_ALEN);
2542 			hdrlen = 24;
2543 		}
2544 		chanctx_conf = rcu_dereference(sdata->vif.chanctx_conf);
2545 		if (!chanctx_conf) {
2546 			ret = -ENOTCONN;
2547 			goto free;
2548 		}
2549 		band = chanctx_conf->def.chan->band;
2550 		break;
2551 	case NL80211_IFTYPE_OCB:
2552 		/* DA SA BSSID */
2553 		memcpy(hdr.addr1, skb->data, ETH_ALEN);
2554 		memcpy(hdr.addr2, skb->data + ETH_ALEN, ETH_ALEN);
2555 		eth_broadcast_addr(hdr.addr3);
2556 		hdrlen = 24;
2557 		chanctx_conf = rcu_dereference(sdata->vif.chanctx_conf);
2558 		if (!chanctx_conf) {
2559 			ret = -ENOTCONN;
2560 			goto free;
2561 		}
2562 		band = chanctx_conf->def.chan->band;
2563 		break;
2564 	case NL80211_IFTYPE_ADHOC:
2565 		/* DA SA BSSID */
2566 		memcpy(hdr.addr1, skb->data, ETH_ALEN);
2567 		memcpy(hdr.addr2, skb->data + ETH_ALEN, ETH_ALEN);
2568 		memcpy(hdr.addr3, sdata->u.ibss.bssid, ETH_ALEN);
2569 		hdrlen = 24;
2570 		chanctx_conf = rcu_dereference(sdata->vif.chanctx_conf);
2571 		if (!chanctx_conf) {
2572 			ret = -ENOTCONN;
2573 			goto free;
2574 		}
2575 		band = chanctx_conf->def.chan->band;
2576 		break;
2577 	default:
2578 		ret = -EINVAL;
2579 		goto free;
2580 	}
2581 
2582 	multicast = is_multicast_ether_addr(hdr.addr1);
2583 
2584 	/* sta is always NULL for mesh */
2585 	if (sta) {
2586 		authorized = test_sta_flag(sta, WLAN_STA_AUTHORIZED);
2587 		wme_sta = sta->sta.wme;
2588 	} else if (ieee80211_vif_is_mesh(&sdata->vif)) {
2589 		/* For mesh, the use of the QoS header is mandatory */
2590 		wme_sta = true;
2591 	}
2592 
2593 	/* receiver does QoS (which also means we do) use it */
2594 	if (wme_sta) {
2595 		fc |= cpu_to_le16(IEEE80211_STYPE_QOS_DATA);
2596 		hdrlen += 2;
2597 	}
2598 
2599 	/*
2600 	 * Drop unicast frames to unauthorised stations unless they are
2601 	 * EAPOL frames from the local station.
2602 	 */
2603 	if (unlikely(!ieee80211_vif_is_mesh(&sdata->vif) &&
2604 		     (sdata->vif.type != NL80211_IFTYPE_OCB) &&
2605 		     !multicast && !authorized &&
2606 		     (cpu_to_be16(ethertype) != sdata->control_port_protocol ||
2607 		      !ether_addr_equal(sdata->vif.addr, skb->data + ETH_ALEN)))) {
2608 #ifdef CONFIG_MAC80211_VERBOSE_DEBUG
2609 		net_info_ratelimited("%s: dropped frame to %pM (unauthorized port)\n",
2610 				    sdata->name, hdr.addr1);
2611 #endif
2612 
2613 		I802_DEBUG_INC(local->tx_handlers_drop_unauth_port);
2614 
2615 		ret = -EPERM;
2616 		goto free;
2617 	}
2618 
2619 	if (unlikely(!multicast && skb->sk &&
2620 		     skb_shinfo(skb)->tx_flags & SKBTX_WIFI_STATUS)) {
2621 		struct sk_buff *ack_skb = skb_clone_sk(skb);
2622 
2623 		if (ack_skb) {
2624 			unsigned long flags;
2625 			int id;
2626 
2627 			spin_lock_irqsave(&local->ack_status_lock, flags);
2628 			id = idr_alloc(&local->ack_status_frames, ack_skb,
2629 				       1, 0x10000, GFP_ATOMIC);
2630 			spin_unlock_irqrestore(&local->ack_status_lock, flags);
2631 
2632 			if (id >= 0) {
2633 				info_id = id;
2634 				info_flags |= IEEE80211_TX_CTL_REQ_TX_STATUS;
2635 			} else {
2636 				kfree_skb(ack_skb);
2637 			}
2638 		}
2639 	}
2640 
2641 	/*
2642 	 * If the skb is shared we need to obtain our own copy.
2643 	 */
2644 	if (skb_shared(skb)) {
2645 		struct sk_buff *tmp_skb = skb;
2646 
2647 		/* can't happen -- skb is a clone if info_id != 0 */
2648 		WARN_ON(info_id);
2649 
2650 		skb = skb_clone(skb, GFP_ATOMIC);
2651 		kfree_skb(tmp_skb);
2652 
2653 		if (!skb) {
2654 			ret = -ENOMEM;
2655 			goto free;
2656 		}
2657 	}
2658 
2659 	hdr.frame_control = fc;
2660 	hdr.duration_id = 0;
2661 	hdr.seq_ctrl = 0;
2662 
2663 	skip_header_bytes = ETH_HLEN;
2664 	if (ethertype == ETH_P_AARP || ethertype == ETH_P_IPX) {
2665 		encaps_data = bridge_tunnel_header;
2666 		encaps_len = sizeof(bridge_tunnel_header);
2667 		skip_header_bytes -= 2;
2668 	} else if (ethertype >= ETH_P_802_3_MIN) {
2669 		encaps_data = rfc1042_header;
2670 		encaps_len = sizeof(rfc1042_header);
2671 		skip_header_bytes -= 2;
2672 	} else {
2673 		encaps_data = NULL;
2674 		encaps_len = 0;
2675 	}
2676 
2677 	skb_pull(skb, skip_header_bytes);
2678 	head_need = hdrlen + encaps_len + meshhdrlen - skb_headroom(skb);
2679 
2680 	/*
2681 	 * So we need to modify the skb header and hence need a copy of
2682 	 * that. The head_need variable above doesn't, so far, include
2683 	 * the needed header space that we don't need right away. If we
2684 	 * can, then we don't reallocate right now but only after the
2685 	 * frame arrives at the master device (if it does...)
2686 	 *
2687 	 * If we cannot, however, then we will reallocate to include all
2688 	 * the ever needed space. Also, if we need to reallocate it anyway,
2689 	 * make it big enough for everything we may ever need.
2690 	 */
2691 
2692 	if (head_need > 0 || skb_cloned(skb)) {
2693 		head_need += sdata->encrypt_headroom;
2694 		head_need += local->tx_headroom;
2695 		head_need = max_t(int, 0, head_need);
2696 		if (ieee80211_skb_resize(sdata, skb, head_need, true)) {
2697 			ieee80211_free_txskb(&local->hw, skb);
2698 			skb = NULL;
2699 			return ERR_PTR(-ENOMEM);
2700 		}
2701 	}
2702 
2703 	if (encaps_data)
2704 		memcpy(skb_push(skb, encaps_len), encaps_data, encaps_len);
2705 
2706 #ifdef CONFIG_MAC80211_MESH
2707 	if (meshhdrlen > 0)
2708 		memcpy(skb_push(skb, meshhdrlen), &mesh_hdr, meshhdrlen);
2709 #endif
2710 
2711 	if (ieee80211_is_data_qos(fc)) {
2712 		__le16 *qos_control;
2713 
2714 		qos_control = (__le16 *) skb_push(skb, 2);
2715 		memcpy(skb_push(skb, hdrlen - 2), &hdr, hdrlen - 2);
2716 		/*
2717 		 * Maybe we could actually set some fields here, for now just
2718 		 * initialise to zero to indicate no special operation.
2719 		 */
2720 		*qos_control = 0;
2721 	} else
2722 		memcpy(skb_push(skb, hdrlen), &hdr, hdrlen);
2723 
2724 	skb_reset_mac_header(skb);
2725 
2726 	info = IEEE80211_SKB_CB(skb);
2727 	memset(info, 0, sizeof(*info));
2728 
2729 	info->flags = info_flags;
2730 	info->ack_frame_id = info_id;
2731 	info->band = band;
2732 
2733 	return skb;
2734  free:
2735 	kfree_skb(skb);
2736 	return ERR_PTR(ret);
2737 }
2738 
2739 /*
2740  * fast-xmit overview
2741  *
2742  * The core idea of this fast-xmit is to remove per-packet checks by checking
2743  * them out of band. ieee80211_check_fast_xmit() implements the out-of-band
2744  * checks that are needed to get the sta->fast_tx pointer assigned, after which
2745  * much less work can be done per packet. For example, fragmentation must be
2746  * disabled or the fast_tx pointer will not be set. All the conditions are seen
2747  * in the code here.
2748  *
2749  * Once assigned, the fast_tx data structure also caches the per-packet 802.11
2750  * header and other data to aid packet processing in ieee80211_xmit_fast().
2751  *
2752  * The most difficult part of this is that when any of these assumptions
2753  * change, an external trigger (i.e. a call to ieee80211_clear_fast_xmit(),
2754  * ieee80211_check_fast_xmit() or friends) is required to reset the data,
2755  * since the per-packet code no longer checks the conditions. This is reflected
2756  * by the calls to these functions throughout the rest of the code, and must be
2757  * maintained if any of the TX path checks change.
2758  */
2759 
2760 void ieee80211_check_fast_xmit(struct sta_info *sta)
2761 {
2762 	struct ieee80211_fast_tx build = {}, *fast_tx = NULL, *old;
2763 	struct ieee80211_local *local = sta->local;
2764 	struct ieee80211_sub_if_data *sdata = sta->sdata;
2765 	struct ieee80211_hdr *hdr = (void *)build.hdr;
2766 	struct ieee80211_chanctx_conf *chanctx_conf;
2767 	__le16 fc;
2768 
2769 	if (!ieee80211_hw_check(&local->hw, SUPPORT_FAST_XMIT))
2770 		return;
2771 
2772 	/* Locking here protects both the pointer itself, and against concurrent
2773 	 * invocations winning data access races to, e.g., the key pointer that
2774 	 * is used.
2775 	 * Without it, the invocation of this function right after the key
2776 	 * pointer changes wouldn't be sufficient, as another CPU could access
2777 	 * the pointer, then stall, and then do the cache update after the CPU
2778 	 * that invalidated the key.
2779 	 * With the locking, such scenarios cannot happen as the check for the
2780 	 * key and the fast-tx assignment are done atomically, so the CPU that
2781 	 * modifies the key will either wait or other one will see the key
2782 	 * cleared/changed already.
2783 	 */
2784 	spin_lock_bh(&sta->lock);
2785 	if (ieee80211_hw_check(&local->hw, SUPPORTS_PS) &&
2786 	    !ieee80211_hw_check(&local->hw, SUPPORTS_DYNAMIC_PS) &&
2787 	    sdata->vif.type == NL80211_IFTYPE_STATION)
2788 		goto out;
2789 
2790 	if (!test_sta_flag(sta, WLAN_STA_AUTHORIZED))
2791 		goto out;
2792 
2793 	if (test_sta_flag(sta, WLAN_STA_PS_STA) ||
2794 	    test_sta_flag(sta, WLAN_STA_PS_DRIVER) ||
2795 	    test_sta_flag(sta, WLAN_STA_PS_DELIVER) ||
2796 	    test_sta_flag(sta, WLAN_STA_CLEAR_PS_FILT))
2797 		goto out;
2798 
2799 	if (sdata->noack_map)
2800 		goto out;
2801 
2802 	/* fast-xmit doesn't handle fragmentation at all */
2803 	if (local->hw.wiphy->frag_threshold != (u32)-1 &&
2804 	    !local->ops->set_frag_threshold)
2805 		goto out;
2806 
2807 	rcu_read_lock();
2808 	chanctx_conf = rcu_dereference(sdata->vif.chanctx_conf);
2809 	if (!chanctx_conf) {
2810 		rcu_read_unlock();
2811 		goto out;
2812 	}
2813 	build.band = chanctx_conf->def.chan->band;
2814 	rcu_read_unlock();
2815 
2816 	fc = cpu_to_le16(IEEE80211_FTYPE_DATA | IEEE80211_STYPE_DATA);
2817 
2818 	switch (sdata->vif.type) {
2819 	case NL80211_IFTYPE_ADHOC:
2820 		/* DA SA BSSID */
2821 		build.da_offs = offsetof(struct ieee80211_hdr, addr1);
2822 		build.sa_offs = offsetof(struct ieee80211_hdr, addr2);
2823 		memcpy(hdr->addr3, sdata->u.ibss.bssid, ETH_ALEN);
2824 		build.hdr_len = 24;
2825 		break;
2826 	case NL80211_IFTYPE_STATION:
2827 		if (test_sta_flag(sta, WLAN_STA_TDLS_PEER)) {
2828 			/* DA SA BSSID */
2829 			build.da_offs = offsetof(struct ieee80211_hdr, addr1);
2830 			build.sa_offs = offsetof(struct ieee80211_hdr, addr2);
2831 			memcpy(hdr->addr3, sdata->u.mgd.bssid, ETH_ALEN);
2832 			build.hdr_len = 24;
2833 			break;
2834 		}
2835 
2836 		if (sdata->u.mgd.use_4addr) {
2837 			/* non-regular ethertype cannot use the fastpath */
2838 			fc |= cpu_to_le16(IEEE80211_FCTL_FROMDS |
2839 					  IEEE80211_FCTL_TODS);
2840 			/* RA TA DA SA */
2841 			memcpy(hdr->addr1, sdata->u.mgd.bssid, ETH_ALEN);
2842 			memcpy(hdr->addr2, sdata->vif.addr, ETH_ALEN);
2843 			build.da_offs = offsetof(struct ieee80211_hdr, addr3);
2844 			build.sa_offs = offsetof(struct ieee80211_hdr, addr4);
2845 			build.hdr_len = 30;
2846 			break;
2847 		}
2848 		fc |= cpu_to_le16(IEEE80211_FCTL_TODS);
2849 		/* BSSID SA DA */
2850 		memcpy(hdr->addr1, sdata->u.mgd.bssid, ETH_ALEN);
2851 		build.da_offs = offsetof(struct ieee80211_hdr, addr3);
2852 		build.sa_offs = offsetof(struct ieee80211_hdr, addr2);
2853 		build.hdr_len = 24;
2854 		break;
2855 	case NL80211_IFTYPE_AP_VLAN:
2856 		if (sdata->wdev.use_4addr) {
2857 			fc |= cpu_to_le16(IEEE80211_FCTL_FROMDS |
2858 					  IEEE80211_FCTL_TODS);
2859 			/* RA TA DA SA */
2860 			memcpy(hdr->addr1, sta->sta.addr, ETH_ALEN);
2861 			memcpy(hdr->addr2, sdata->vif.addr, ETH_ALEN);
2862 			build.da_offs = offsetof(struct ieee80211_hdr, addr3);
2863 			build.sa_offs = offsetof(struct ieee80211_hdr, addr4);
2864 			build.hdr_len = 30;
2865 			break;
2866 		}
2867 		/* fall through */
2868 	case NL80211_IFTYPE_AP:
2869 		fc |= cpu_to_le16(IEEE80211_FCTL_FROMDS);
2870 		/* DA BSSID SA */
2871 		build.da_offs = offsetof(struct ieee80211_hdr, addr1);
2872 		memcpy(hdr->addr2, sdata->vif.addr, ETH_ALEN);
2873 		build.sa_offs = offsetof(struct ieee80211_hdr, addr3);
2874 		build.hdr_len = 24;
2875 		break;
2876 	default:
2877 		/* not handled on fast-xmit */
2878 		goto out;
2879 	}
2880 
2881 	if (sta->sta.wme) {
2882 		build.hdr_len += 2;
2883 		fc |= cpu_to_le16(IEEE80211_STYPE_QOS_DATA);
2884 	}
2885 
2886 	/* We store the key here so there's no point in using rcu_dereference()
2887 	 * but that's fine because the code that changes the pointers will call
2888 	 * this function after doing so. For a single CPU that would be enough,
2889 	 * for multiple see the comment above.
2890 	 */
2891 	build.key = rcu_access_pointer(sta->ptk[sta->ptk_idx]);
2892 	if (!build.key)
2893 		build.key = rcu_access_pointer(sdata->default_unicast_key);
2894 	if (build.key) {
2895 		bool gen_iv, iv_spc, mmic;
2896 
2897 		gen_iv = build.key->conf.flags & IEEE80211_KEY_FLAG_GENERATE_IV;
2898 		iv_spc = build.key->conf.flags & IEEE80211_KEY_FLAG_PUT_IV_SPACE;
2899 		mmic = build.key->conf.flags & IEEE80211_KEY_FLAG_GENERATE_MMIC;
2900 
2901 		/* don't handle software crypto */
2902 		if (!(build.key->flags & KEY_FLAG_UPLOADED_TO_HARDWARE))
2903 			goto out;
2904 
2905 		switch (build.key->conf.cipher) {
2906 		case WLAN_CIPHER_SUITE_CCMP:
2907 		case WLAN_CIPHER_SUITE_CCMP_256:
2908 			/* add fixed key ID */
2909 			if (gen_iv) {
2910 				(build.hdr + build.hdr_len)[3] =
2911 					0x20 | (build.key->conf.keyidx << 6);
2912 				build.pn_offs = build.hdr_len;
2913 			}
2914 			if (gen_iv || iv_spc)
2915 				build.hdr_len += IEEE80211_CCMP_HDR_LEN;
2916 			break;
2917 		case WLAN_CIPHER_SUITE_GCMP:
2918 		case WLAN_CIPHER_SUITE_GCMP_256:
2919 			/* add fixed key ID */
2920 			if (gen_iv) {
2921 				(build.hdr + build.hdr_len)[3] =
2922 					0x20 | (build.key->conf.keyidx << 6);
2923 				build.pn_offs = build.hdr_len;
2924 			}
2925 			if (gen_iv || iv_spc)
2926 				build.hdr_len += IEEE80211_GCMP_HDR_LEN;
2927 			break;
2928 		case WLAN_CIPHER_SUITE_TKIP:
2929 			/* cannot handle MMIC or IV generation in xmit-fast */
2930 			if (mmic || gen_iv)
2931 				goto out;
2932 			if (iv_spc)
2933 				build.hdr_len += IEEE80211_TKIP_IV_LEN;
2934 			break;
2935 		case WLAN_CIPHER_SUITE_WEP40:
2936 		case WLAN_CIPHER_SUITE_WEP104:
2937 			/* cannot handle IV generation in fast-xmit */
2938 			if (gen_iv)
2939 				goto out;
2940 			if (iv_spc)
2941 				build.hdr_len += IEEE80211_WEP_IV_LEN;
2942 			break;
2943 		case WLAN_CIPHER_SUITE_AES_CMAC:
2944 		case WLAN_CIPHER_SUITE_BIP_CMAC_256:
2945 		case WLAN_CIPHER_SUITE_BIP_GMAC_128:
2946 		case WLAN_CIPHER_SUITE_BIP_GMAC_256:
2947 			WARN(1,
2948 			     "management cipher suite 0x%x enabled for data\n",
2949 			     build.key->conf.cipher);
2950 			goto out;
2951 		default:
2952 			/* we don't know how to generate IVs for this at all */
2953 			if (WARN_ON(gen_iv))
2954 				goto out;
2955 			/* pure hardware keys are OK, of course */
2956 			if (!(build.key->flags & KEY_FLAG_CIPHER_SCHEME))
2957 				break;
2958 			/* cipher scheme might require space allocation */
2959 			if (iv_spc &&
2960 			    build.key->conf.iv_len > IEEE80211_FAST_XMIT_MAX_IV)
2961 				goto out;
2962 			if (iv_spc)
2963 				build.hdr_len += build.key->conf.iv_len;
2964 		}
2965 
2966 		fc |= cpu_to_le16(IEEE80211_FCTL_PROTECTED);
2967 	}
2968 
2969 	hdr->frame_control = fc;
2970 
2971 	memcpy(build.hdr + build.hdr_len,
2972 	       rfc1042_header,  sizeof(rfc1042_header));
2973 	build.hdr_len += sizeof(rfc1042_header);
2974 
2975 	fast_tx = kmemdup(&build, sizeof(build), GFP_ATOMIC);
2976 	/* if the kmemdup fails, continue w/o fast_tx */
2977 	if (!fast_tx)
2978 		goto out;
2979 
2980  out:
2981 	/* we might have raced against another call to this function */
2982 	old = rcu_dereference_protected(sta->fast_tx,
2983 					lockdep_is_held(&sta->lock));
2984 	rcu_assign_pointer(sta->fast_tx, fast_tx);
2985 	if (old)
2986 		kfree_rcu(old, rcu_head);
2987 	spin_unlock_bh(&sta->lock);
2988 }
2989 
2990 void ieee80211_check_fast_xmit_all(struct ieee80211_local *local)
2991 {
2992 	struct sta_info *sta;
2993 
2994 	rcu_read_lock();
2995 	list_for_each_entry_rcu(sta, &local->sta_list, list)
2996 		ieee80211_check_fast_xmit(sta);
2997 	rcu_read_unlock();
2998 }
2999 
3000 void ieee80211_check_fast_xmit_iface(struct ieee80211_sub_if_data *sdata)
3001 {
3002 	struct ieee80211_local *local = sdata->local;
3003 	struct sta_info *sta;
3004 
3005 	rcu_read_lock();
3006 
3007 	list_for_each_entry_rcu(sta, &local->sta_list, list) {
3008 		if (sdata != sta->sdata &&
3009 		    (!sta->sdata->bss || sta->sdata->bss != sdata->bss))
3010 			continue;
3011 		ieee80211_check_fast_xmit(sta);
3012 	}
3013 
3014 	rcu_read_unlock();
3015 }
3016 
3017 void ieee80211_clear_fast_xmit(struct sta_info *sta)
3018 {
3019 	struct ieee80211_fast_tx *fast_tx;
3020 
3021 	spin_lock_bh(&sta->lock);
3022 	fast_tx = rcu_dereference_protected(sta->fast_tx,
3023 					    lockdep_is_held(&sta->lock));
3024 	RCU_INIT_POINTER(sta->fast_tx, NULL);
3025 	spin_unlock_bh(&sta->lock);
3026 
3027 	if (fast_tx)
3028 		kfree_rcu(fast_tx, rcu_head);
3029 }
3030 
3031 static bool ieee80211_amsdu_realloc_pad(struct ieee80211_local *local,
3032 					struct sk_buff *skb, int headroom,
3033 					int *subframe_len)
3034 {
3035 	int amsdu_len = *subframe_len + sizeof(struct ethhdr);
3036 	int padding = (4 - amsdu_len) & 3;
3037 
3038 	if (skb_headroom(skb) < headroom || skb_tailroom(skb) < padding) {
3039 		I802_DEBUG_INC(local->tx_expand_skb_head);
3040 
3041 		if (pskb_expand_head(skb, headroom, padding, GFP_ATOMIC)) {
3042 			wiphy_debug(local->hw.wiphy,
3043 				    "failed to reallocate TX buffer\n");
3044 			return false;
3045 		}
3046 	}
3047 
3048 	if (padding) {
3049 		*subframe_len += padding;
3050 		memset(skb_put(skb, padding), 0, padding);
3051 	}
3052 
3053 	return true;
3054 }
3055 
3056 static bool ieee80211_amsdu_prepare_head(struct ieee80211_sub_if_data *sdata,
3057 					 struct ieee80211_fast_tx *fast_tx,
3058 					 struct sk_buff *skb)
3059 {
3060 	struct ieee80211_local *local = sdata->local;
3061 	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
3062 	struct ieee80211_hdr *hdr;
3063 	struct ethhdr amsdu_hdr;
3064 	int hdr_len = fast_tx->hdr_len - sizeof(rfc1042_header);
3065 	int subframe_len = skb->len - hdr_len;
3066 	void *data;
3067 	u8 *qc;
3068 
3069 	if (info->flags & IEEE80211_TX_CTL_RATE_CTRL_PROBE)
3070 		return false;
3071 
3072 	if (info->control.flags & IEEE80211_TX_CTRL_AMSDU)
3073 		return true;
3074 
3075 	if (!ieee80211_amsdu_realloc_pad(local, skb, sizeof(amsdu_hdr),
3076 					 &subframe_len))
3077 		return false;
3078 
3079 	amsdu_hdr.h_proto = cpu_to_be16(subframe_len);
3080 	memcpy(amsdu_hdr.h_source, skb->data + fast_tx->sa_offs, ETH_ALEN);
3081 	memcpy(amsdu_hdr.h_dest, skb->data + fast_tx->da_offs, ETH_ALEN);
3082 
3083 	data = skb_push(skb, sizeof(amsdu_hdr));
3084 	memmove(data, data + sizeof(amsdu_hdr), hdr_len);
3085 	memcpy(data + hdr_len, &amsdu_hdr, sizeof(amsdu_hdr));
3086 
3087 	hdr = data;
3088 	qc = ieee80211_get_qos_ctl(hdr);
3089 	*qc |= IEEE80211_QOS_CTL_A_MSDU_PRESENT;
3090 
3091 	info->control.flags |= IEEE80211_TX_CTRL_AMSDU;
3092 
3093 	return true;
3094 }
3095 
3096 static bool ieee80211_amsdu_aggregate(struct ieee80211_sub_if_data *sdata,
3097 				      struct sta_info *sta,
3098 				      struct ieee80211_fast_tx *fast_tx,
3099 				      struct sk_buff *skb)
3100 {
3101 	struct ieee80211_local *local = sdata->local;
3102 	struct fq *fq = &local->fq;
3103 	struct fq_tin *tin;
3104 	struct fq_flow *flow;
3105 	u8 tid = skb->priority & IEEE80211_QOS_CTL_TAG1D_MASK;
3106 	struct ieee80211_txq *txq = sta->sta.txq[tid];
3107 	struct txq_info *txqi;
3108 	struct sk_buff **frag_tail, *head;
3109 	int subframe_len = skb->len - ETH_ALEN;
3110 	u8 max_subframes = sta->sta.max_amsdu_subframes;
3111 	int max_frags = local->hw.max_tx_fragments;
3112 	int max_amsdu_len = sta->sta.max_amsdu_len;
3113 	__be16 len;
3114 	void *data;
3115 	bool ret = false;
3116 	unsigned int orig_len;
3117 	int n = 1, nfrags;
3118 
3119 	if (!ieee80211_hw_check(&local->hw, TX_AMSDU))
3120 		return false;
3121 
3122 	if (!txq)
3123 		return false;
3124 
3125 	txqi = to_txq_info(txq);
3126 	if (test_bit(IEEE80211_TXQ_NO_AMSDU, &txqi->flags))
3127 		return false;
3128 
3129 	if (sta->sta.max_rc_amsdu_len)
3130 		max_amsdu_len = min_t(int, max_amsdu_len,
3131 				      sta->sta.max_rc_amsdu_len);
3132 
3133 	spin_lock_bh(&fq->lock);
3134 
3135 	/* TODO: Ideally aggregation should be done on dequeue to remain
3136 	 * responsive to environment changes.
3137 	 */
3138 
3139 	tin = &txqi->tin;
3140 	flow = fq_flow_classify(fq, tin, skb, fq_flow_get_default_func);
3141 	head = skb_peek_tail(&flow->queue);
3142 	if (!head)
3143 		goto out;
3144 
3145 	orig_len = head->len;
3146 
3147 	if (skb->len + head->len > max_amsdu_len)
3148 		goto out;
3149 
3150 	if (!ieee80211_amsdu_prepare_head(sdata, fast_tx, head))
3151 		goto out;
3152 
3153 	nfrags = 1 + skb_shinfo(skb)->nr_frags;
3154 	nfrags += 1 + skb_shinfo(head)->nr_frags;
3155 	frag_tail = &skb_shinfo(head)->frag_list;
3156 	while (*frag_tail) {
3157 		nfrags += 1 + skb_shinfo(*frag_tail)->nr_frags;
3158 		frag_tail = &(*frag_tail)->next;
3159 		n++;
3160 	}
3161 
3162 	if (max_subframes && n > max_subframes)
3163 		goto out;
3164 
3165 	if (max_frags && nfrags > max_frags)
3166 		goto out;
3167 
3168 	if (!ieee80211_amsdu_realloc_pad(local, skb, sizeof(rfc1042_header) + 2,
3169 					 &subframe_len))
3170 		goto out;
3171 
3172 	ret = true;
3173 	data = skb_push(skb, ETH_ALEN + 2);
3174 	memmove(data, data + ETH_ALEN + 2, 2 * ETH_ALEN);
3175 
3176 	data += 2 * ETH_ALEN;
3177 	len = cpu_to_be16(subframe_len);
3178 	memcpy(data, &len, 2);
3179 	memcpy(data + 2, rfc1042_header, sizeof(rfc1042_header));
3180 
3181 	head->len += skb->len;
3182 	head->data_len += skb->len;
3183 	*frag_tail = skb;
3184 
3185 	flow->backlog += head->len - orig_len;
3186 	tin->backlog_bytes += head->len - orig_len;
3187 
3188 	fq_recalc_backlog(fq, tin, flow);
3189 
3190 out:
3191 	spin_unlock_bh(&fq->lock);
3192 
3193 	return ret;
3194 }
3195 
3196 /*
3197  * Can be called while the sta lock is held. Anything that can cause packets to
3198  * be generated will cause deadlock!
3199  */
3200 static void ieee80211_xmit_fast_finish(struct ieee80211_sub_if_data *sdata,
3201 				       struct sta_info *sta, u8 pn_offs,
3202 				       struct ieee80211_key *key,
3203 				       struct sk_buff *skb)
3204 {
3205 	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
3206 	struct ieee80211_hdr *hdr = (void *)skb->data;
3207 	u8 tid = IEEE80211_NUM_TIDS;
3208 
3209 	if (key)
3210 		info->control.hw_key = &key->conf;
3211 
3212 	ieee80211_tx_stats(skb->dev, skb->len);
3213 
3214 	if (hdr->frame_control & cpu_to_le16(IEEE80211_STYPE_QOS_DATA)) {
3215 		tid = skb->priority & IEEE80211_QOS_CTL_TAG1D_MASK;
3216 		*ieee80211_get_qos_ctl(hdr) = tid;
3217 		hdr->seq_ctrl = ieee80211_tx_next_seq(sta, tid);
3218 	} else {
3219 		info->flags |= IEEE80211_TX_CTL_ASSIGN_SEQ;
3220 		hdr->seq_ctrl = cpu_to_le16(sdata->sequence_number);
3221 		sdata->sequence_number += 0x10;
3222 	}
3223 
3224 	if (skb_shinfo(skb)->gso_size)
3225 		sta->tx_stats.msdu[tid] +=
3226 			DIV_ROUND_UP(skb->len, skb_shinfo(skb)->gso_size);
3227 	else
3228 		sta->tx_stats.msdu[tid]++;
3229 
3230 	info->hw_queue = sdata->vif.hw_queue[skb_get_queue_mapping(skb)];
3231 
3232 	/* statistics normally done by ieee80211_tx_h_stats (but that
3233 	 * has to consider fragmentation, so is more complex)
3234 	 */
3235 	sta->tx_stats.bytes[skb_get_queue_mapping(skb)] += skb->len;
3236 	sta->tx_stats.packets[skb_get_queue_mapping(skb)]++;
3237 
3238 	if (pn_offs) {
3239 		u64 pn;
3240 		u8 *crypto_hdr = skb->data + pn_offs;
3241 
3242 		switch (key->conf.cipher) {
3243 		case WLAN_CIPHER_SUITE_CCMP:
3244 		case WLAN_CIPHER_SUITE_CCMP_256:
3245 		case WLAN_CIPHER_SUITE_GCMP:
3246 		case WLAN_CIPHER_SUITE_GCMP_256:
3247 			pn = atomic64_inc_return(&key->conf.tx_pn);
3248 			crypto_hdr[0] = pn;
3249 			crypto_hdr[1] = pn >> 8;
3250 			crypto_hdr[4] = pn >> 16;
3251 			crypto_hdr[5] = pn >> 24;
3252 			crypto_hdr[6] = pn >> 32;
3253 			crypto_hdr[7] = pn >> 40;
3254 			break;
3255 		}
3256 	}
3257 }
3258 
3259 static bool ieee80211_xmit_fast(struct ieee80211_sub_if_data *sdata,
3260 				struct sta_info *sta,
3261 				struct ieee80211_fast_tx *fast_tx,
3262 				struct sk_buff *skb)
3263 {
3264 	struct ieee80211_local *local = sdata->local;
3265 	u16 ethertype = (skb->data[12] << 8) | skb->data[13];
3266 	int extra_head = fast_tx->hdr_len - (ETH_HLEN - 2);
3267 	int hw_headroom = sdata->local->hw.extra_tx_headroom;
3268 	struct ethhdr eth;
3269 	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
3270 	struct ieee80211_hdr *hdr = (void *)fast_tx->hdr;
3271 	struct ieee80211_tx_data tx;
3272 	ieee80211_tx_result r;
3273 	struct tid_ampdu_tx *tid_tx = NULL;
3274 	u8 tid = IEEE80211_NUM_TIDS;
3275 
3276 	/* control port protocol needs a lot of special handling */
3277 	if (cpu_to_be16(ethertype) == sdata->control_port_protocol)
3278 		return false;
3279 
3280 	/* only RFC 1042 SNAP */
3281 	if (ethertype < ETH_P_802_3_MIN)
3282 		return false;
3283 
3284 	/* don't handle TX status request here either */
3285 	if (skb->sk && skb_shinfo(skb)->tx_flags & SKBTX_WIFI_STATUS)
3286 		return false;
3287 
3288 	if (hdr->frame_control & cpu_to_le16(IEEE80211_STYPE_QOS_DATA)) {
3289 		tid = skb->priority & IEEE80211_QOS_CTL_TAG1D_MASK;
3290 		tid_tx = rcu_dereference(sta->ampdu_mlme.tid_tx[tid]);
3291 		if (tid_tx) {
3292 			if (!test_bit(HT_AGG_STATE_OPERATIONAL, &tid_tx->state))
3293 				return false;
3294 			if (tid_tx->timeout)
3295 				tid_tx->last_tx = jiffies;
3296 		}
3297 	}
3298 
3299 	/* after this point (skb is modified) we cannot return false */
3300 
3301 	if (skb_shared(skb)) {
3302 		struct sk_buff *tmp_skb = skb;
3303 
3304 		skb = skb_clone(skb, GFP_ATOMIC);
3305 		kfree_skb(tmp_skb);
3306 
3307 		if (!skb)
3308 			return true;
3309 	}
3310 
3311 	if ((hdr->frame_control & cpu_to_le16(IEEE80211_STYPE_QOS_DATA)) &&
3312 	    ieee80211_amsdu_aggregate(sdata, sta, fast_tx, skb))
3313 		return true;
3314 
3315 	/* will not be crypto-handled beyond what we do here, so use false
3316 	 * as the may-encrypt argument for the resize to not account for
3317 	 * more room than we already have in 'extra_head'
3318 	 */
3319 	if (unlikely(ieee80211_skb_resize(sdata, skb,
3320 					  max_t(int, extra_head + hw_headroom -
3321 						     skb_headroom(skb), 0),
3322 					  false))) {
3323 		kfree_skb(skb);
3324 		return true;
3325 	}
3326 
3327 	memcpy(&eth, skb->data, ETH_HLEN - 2);
3328 	hdr = (void *)skb_push(skb, extra_head);
3329 	memcpy(skb->data, fast_tx->hdr, fast_tx->hdr_len);
3330 	memcpy(skb->data + fast_tx->da_offs, eth.h_dest, ETH_ALEN);
3331 	memcpy(skb->data + fast_tx->sa_offs, eth.h_source, ETH_ALEN);
3332 
3333 	memset(info, 0, sizeof(*info));
3334 	info->band = fast_tx->band;
3335 	info->control.vif = &sdata->vif;
3336 	info->flags = IEEE80211_TX_CTL_FIRST_FRAGMENT |
3337 		      IEEE80211_TX_CTL_DONTFRAG |
3338 		      (tid_tx ? IEEE80211_TX_CTL_AMPDU : 0);
3339 	info->control.flags = IEEE80211_TX_CTRL_FAST_XMIT;
3340 
3341 	__skb_queue_head_init(&tx.skbs);
3342 
3343 	tx.flags = IEEE80211_TX_UNICAST;
3344 	tx.local = local;
3345 	tx.sdata = sdata;
3346 	tx.sta = sta;
3347 	tx.key = fast_tx->key;
3348 
3349 	if (!ieee80211_hw_check(&local->hw, HAS_RATE_CONTROL)) {
3350 		tx.skb = skb;
3351 		r = ieee80211_tx_h_rate_ctrl(&tx);
3352 		skb = tx.skb;
3353 		tx.skb = NULL;
3354 
3355 		if (r != TX_CONTINUE) {
3356 			if (r != TX_QUEUED)
3357 				kfree_skb(skb);
3358 			return true;
3359 		}
3360 	}
3361 
3362 	if (ieee80211_queue_skb(local, sdata, sta, skb))
3363 		return true;
3364 
3365 	ieee80211_xmit_fast_finish(sdata, sta, fast_tx->pn_offs,
3366 				   fast_tx->key, skb);
3367 
3368 	if (sdata->vif.type == NL80211_IFTYPE_AP_VLAN)
3369 		sdata = container_of(sdata->bss,
3370 				     struct ieee80211_sub_if_data, u.ap);
3371 
3372 	__skb_queue_tail(&tx.skbs, skb);
3373 	ieee80211_tx_frags(local, &sdata->vif, &sta->sta, &tx.skbs, false);
3374 	return true;
3375 }
3376 
3377 struct sk_buff *ieee80211_tx_dequeue(struct ieee80211_hw *hw,
3378 				     struct ieee80211_txq *txq)
3379 {
3380 	struct ieee80211_local *local = hw_to_local(hw);
3381 	struct txq_info *txqi = container_of(txq, struct txq_info, txq);
3382 	struct ieee80211_hdr *hdr;
3383 	struct sk_buff *skb = NULL;
3384 	struct fq *fq = &local->fq;
3385 	struct fq_tin *tin = &txqi->tin;
3386 	struct ieee80211_tx_info *info;
3387 	struct ieee80211_tx_data tx;
3388 	ieee80211_tx_result r;
3389 
3390 	spin_lock_bh(&fq->lock);
3391 
3392 	if (test_bit(IEEE80211_TXQ_STOP, &txqi->flags))
3393 		goto out;
3394 
3395 	/* Make sure fragments stay together. */
3396 	skb = __skb_dequeue(&txqi->frags);
3397 	if (skb)
3398 		goto out;
3399 
3400 begin:
3401 	skb = fq_tin_dequeue(fq, tin, fq_tin_dequeue_func);
3402 	if (!skb)
3403 		goto out;
3404 
3405 	ieee80211_set_skb_vif(skb, txqi);
3406 
3407 	hdr = (struct ieee80211_hdr *)skb->data;
3408 	info = IEEE80211_SKB_CB(skb);
3409 
3410 	memset(&tx, 0, sizeof(tx));
3411 	__skb_queue_head_init(&tx.skbs);
3412 	tx.local = local;
3413 	tx.skb = skb;
3414 	tx.sdata = vif_to_sdata(info->control.vif);
3415 
3416 	if (txq->sta)
3417 		tx.sta = container_of(txq->sta, struct sta_info, sta);
3418 
3419 	/*
3420 	 * The key can be removed while the packet was queued, so need to call
3421 	 * this here to get the current key.
3422 	 */
3423 	r = ieee80211_tx_h_select_key(&tx);
3424 	if (r != TX_CONTINUE) {
3425 		ieee80211_free_txskb(&local->hw, skb);
3426 		goto begin;
3427 	}
3428 
3429 	if (info->control.flags & IEEE80211_TX_CTRL_FAST_XMIT) {
3430 		struct sta_info *sta = container_of(txq->sta, struct sta_info,
3431 						    sta);
3432 		u8 pn_offs = 0;
3433 
3434 		if (tx.key &&
3435 		    (tx.key->conf.flags & IEEE80211_KEY_FLAG_GENERATE_IV))
3436 			pn_offs = ieee80211_hdrlen(hdr->frame_control);
3437 
3438 		ieee80211_xmit_fast_finish(sta->sdata, sta, pn_offs,
3439 					   tx.key, skb);
3440 	} else {
3441 		if (invoke_tx_handlers_late(&tx))
3442 			goto begin;
3443 
3444 		skb = __skb_dequeue(&tx.skbs);
3445 
3446 		if (!skb_queue_empty(&tx.skbs))
3447 			skb_queue_splice_tail(&tx.skbs, &txqi->frags);
3448 	}
3449 
3450 	if (skb && skb_has_frag_list(skb) &&
3451 	    !ieee80211_hw_check(&local->hw, TX_FRAG_LIST)) {
3452 		if (skb_linearize(skb)) {
3453 			ieee80211_free_txskb(&local->hw, skb);
3454 			goto begin;
3455 		}
3456 	}
3457 
3458 out:
3459 	spin_unlock_bh(&fq->lock);
3460 
3461 	return skb;
3462 }
3463 EXPORT_SYMBOL(ieee80211_tx_dequeue);
3464 
3465 void __ieee80211_subif_start_xmit(struct sk_buff *skb,
3466 				  struct net_device *dev,
3467 				  u32 info_flags)
3468 {
3469 	struct ieee80211_sub_if_data *sdata = IEEE80211_DEV_TO_SUB_IF(dev);
3470 	struct sta_info *sta;
3471 	struct sk_buff *next;
3472 
3473 	if (unlikely(skb->len < ETH_HLEN)) {
3474 		kfree_skb(skb);
3475 		return;
3476 	}
3477 
3478 	rcu_read_lock();
3479 
3480 	if (ieee80211_lookup_ra_sta(sdata, skb, &sta))
3481 		goto out_free;
3482 
3483 	if (!IS_ERR_OR_NULL(sta)) {
3484 		struct ieee80211_fast_tx *fast_tx;
3485 
3486 		fast_tx = rcu_dereference(sta->fast_tx);
3487 
3488 		if (fast_tx &&
3489 		    ieee80211_xmit_fast(sdata, sta, fast_tx, skb))
3490 			goto out;
3491 	}
3492 
3493 	if (skb_is_gso(skb)) {
3494 		struct sk_buff *segs;
3495 
3496 		segs = skb_gso_segment(skb, 0);
3497 		if (IS_ERR(segs)) {
3498 			goto out_free;
3499 		} else if (segs) {
3500 			consume_skb(skb);
3501 			skb = segs;
3502 		}
3503 	} else {
3504 		/* we cannot process non-linear frames on this path */
3505 		if (skb_linearize(skb)) {
3506 			kfree_skb(skb);
3507 			goto out;
3508 		}
3509 
3510 		/* the frame could be fragmented, software-encrypted, and other
3511 		 * things so we cannot really handle checksum offload with it -
3512 		 * fix it up in software before we handle anything else.
3513 		 */
3514 		if (skb->ip_summed == CHECKSUM_PARTIAL) {
3515 			skb_set_transport_header(skb,
3516 						 skb_checksum_start_offset(skb));
3517 			if (skb_checksum_help(skb))
3518 				goto out_free;
3519 		}
3520 	}
3521 
3522 	next = skb;
3523 	while (next) {
3524 		skb = next;
3525 		next = skb->next;
3526 
3527 		skb->prev = NULL;
3528 		skb->next = NULL;
3529 
3530 		skb = ieee80211_build_hdr(sdata, skb, info_flags, sta);
3531 		if (IS_ERR(skb))
3532 			goto out;
3533 
3534 		ieee80211_tx_stats(dev, skb->len);
3535 
3536 		ieee80211_xmit(sdata, sta, skb);
3537 	}
3538 	goto out;
3539  out_free:
3540 	kfree_skb(skb);
3541  out:
3542 	rcu_read_unlock();
3543 }
3544 
3545 /**
3546  * ieee80211_subif_start_xmit - netif start_xmit function for 802.3 vifs
3547  * @skb: packet to be sent
3548  * @dev: incoming interface
3549  *
3550  * On failure skb will be freed.
3551  */
3552 netdev_tx_t ieee80211_subif_start_xmit(struct sk_buff *skb,
3553 				       struct net_device *dev)
3554 {
3555 	__ieee80211_subif_start_xmit(skb, dev, 0);
3556 	return NETDEV_TX_OK;
3557 }
3558 
3559 struct sk_buff *
3560 ieee80211_build_data_template(struct ieee80211_sub_if_data *sdata,
3561 			      struct sk_buff *skb, u32 info_flags)
3562 {
3563 	struct ieee80211_hdr *hdr;
3564 	struct ieee80211_tx_data tx = {
3565 		.local = sdata->local,
3566 		.sdata = sdata,
3567 	};
3568 	struct sta_info *sta;
3569 
3570 	rcu_read_lock();
3571 
3572 	if (ieee80211_lookup_ra_sta(sdata, skb, &sta)) {
3573 		kfree_skb(skb);
3574 		skb = ERR_PTR(-EINVAL);
3575 		goto out;
3576 	}
3577 
3578 	skb = ieee80211_build_hdr(sdata, skb, info_flags, sta);
3579 	if (IS_ERR(skb))
3580 		goto out;
3581 
3582 	hdr = (void *)skb->data;
3583 	tx.sta = sta_info_get(sdata, hdr->addr1);
3584 	tx.skb = skb;
3585 
3586 	if (ieee80211_tx_h_select_key(&tx) != TX_CONTINUE) {
3587 		rcu_read_unlock();
3588 		kfree_skb(skb);
3589 		return ERR_PTR(-EINVAL);
3590 	}
3591 
3592 out:
3593 	rcu_read_unlock();
3594 	return skb;
3595 }
3596 
3597 /*
3598  * ieee80211_clear_tx_pending may not be called in a context where
3599  * it is possible that it packets could come in again.
3600  */
3601 void ieee80211_clear_tx_pending(struct ieee80211_local *local)
3602 {
3603 	struct sk_buff *skb;
3604 	int i;
3605 
3606 	for (i = 0; i < local->hw.queues; i++) {
3607 		while ((skb = skb_dequeue(&local->pending[i])) != NULL)
3608 			ieee80211_free_txskb(&local->hw, skb);
3609 	}
3610 }
3611 
3612 /*
3613  * Returns false if the frame couldn't be transmitted but was queued instead,
3614  * which in this case means re-queued -- take as an indication to stop sending
3615  * more pending frames.
3616  */
3617 static bool ieee80211_tx_pending_skb(struct ieee80211_local *local,
3618 				     struct sk_buff *skb)
3619 {
3620 	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
3621 	struct ieee80211_sub_if_data *sdata;
3622 	struct sta_info *sta;
3623 	struct ieee80211_hdr *hdr;
3624 	bool result;
3625 	struct ieee80211_chanctx_conf *chanctx_conf;
3626 
3627 	sdata = vif_to_sdata(info->control.vif);
3628 
3629 	if (info->flags & IEEE80211_TX_INTFL_NEED_TXPROCESSING) {
3630 		chanctx_conf = rcu_dereference(sdata->vif.chanctx_conf);
3631 		if (unlikely(!chanctx_conf)) {
3632 			dev_kfree_skb(skb);
3633 			return true;
3634 		}
3635 		info->band = chanctx_conf->def.chan->band;
3636 		result = ieee80211_tx(sdata, NULL, skb, true);
3637 	} else {
3638 		struct sk_buff_head skbs;
3639 
3640 		__skb_queue_head_init(&skbs);
3641 		__skb_queue_tail(&skbs, skb);
3642 
3643 		hdr = (struct ieee80211_hdr *)skb->data;
3644 		sta = sta_info_get(sdata, hdr->addr1);
3645 
3646 		result = __ieee80211_tx(local, &skbs, skb->len, sta, true);
3647 	}
3648 
3649 	return result;
3650 }
3651 
3652 /*
3653  * Transmit all pending packets. Called from tasklet.
3654  */
3655 void ieee80211_tx_pending(unsigned long data)
3656 {
3657 	struct ieee80211_local *local = (struct ieee80211_local *)data;
3658 	unsigned long flags;
3659 	int i;
3660 	bool txok;
3661 
3662 	rcu_read_lock();
3663 
3664 	spin_lock_irqsave(&local->queue_stop_reason_lock, flags);
3665 	for (i = 0; i < local->hw.queues; i++) {
3666 		/*
3667 		 * If queue is stopped by something other than due to pending
3668 		 * frames, or we have no pending frames, proceed to next queue.
3669 		 */
3670 		if (local->queue_stop_reasons[i] ||
3671 		    skb_queue_empty(&local->pending[i]))
3672 			continue;
3673 
3674 		while (!skb_queue_empty(&local->pending[i])) {
3675 			struct sk_buff *skb = __skb_dequeue(&local->pending[i]);
3676 			struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
3677 
3678 			if (WARN_ON(!info->control.vif)) {
3679 				ieee80211_free_txskb(&local->hw, skb);
3680 				continue;
3681 			}
3682 
3683 			spin_unlock_irqrestore(&local->queue_stop_reason_lock,
3684 						flags);
3685 
3686 			txok = ieee80211_tx_pending_skb(local, skb);
3687 			spin_lock_irqsave(&local->queue_stop_reason_lock,
3688 					  flags);
3689 			if (!txok)
3690 				break;
3691 		}
3692 
3693 		if (skb_queue_empty(&local->pending[i]))
3694 			ieee80211_propagate_queue_wake(local, i);
3695 	}
3696 	spin_unlock_irqrestore(&local->queue_stop_reason_lock, flags);
3697 
3698 	rcu_read_unlock();
3699 }
3700 
3701 /* functions for drivers to get certain frames */
3702 
3703 static void __ieee80211_beacon_add_tim(struct ieee80211_sub_if_data *sdata,
3704 				       struct ps_data *ps, struct sk_buff *skb,
3705 				       bool is_template)
3706 {
3707 	u8 *pos, *tim;
3708 	int aid0 = 0;
3709 	int i, have_bits = 0, n1, n2;
3710 
3711 	/* Generate bitmap for TIM only if there are any STAs in power save
3712 	 * mode. */
3713 	if (atomic_read(&ps->num_sta_ps) > 0)
3714 		/* in the hope that this is faster than
3715 		 * checking byte-for-byte */
3716 		have_bits = !bitmap_empty((unsigned long *)ps->tim,
3717 					  IEEE80211_MAX_AID+1);
3718 	if (!is_template) {
3719 		if (ps->dtim_count == 0)
3720 			ps->dtim_count = sdata->vif.bss_conf.dtim_period - 1;
3721 		else
3722 			ps->dtim_count--;
3723 	}
3724 
3725 	tim = pos = (u8 *) skb_put(skb, 6);
3726 	*pos++ = WLAN_EID_TIM;
3727 	*pos++ = 4;
3728 	*pos++ = ps->dtim_count;
3729 	*pos++ = sdata->vif.bss_conf.dtim_period;
3730 
3731 	if (ps->dtim_count == 0 && !skb_queue_empty(&ps->bc_buf))
3732 		aid0 = 1;
3733 
3734 	ps->dtim_bc_mc = aid0 == 1;
3735 
3736 	if (have_bits) {
3737 		/* Find largest even number N1 so that bits numbered 1 through
3738 		 * (N1 x 8) - 1 in the bitmap are 0 and number N2 so that bits
3739 		 * (N2 + 1) x 8 through 2007 are 0. */
3740 		n1 = 0;
3741 		for (i = 0; i < IEEE80211_MAX_TIM_LEN; i++) {
3742 			if (ps->tim[i]) {
3743 				n1 = i & 0xfe;
3744 				break;
3745 			}
3746 		}
3747 		n2 = n1;
3748 		for (i = IEEE80211_MAX_TIM_LEN - 1; i >= n1; i--) {
3749 			if (ps->tim[i]) {
3750 				n2 = i;
3751 				break;
3752 			}
3753 		}
3754 
3755 		/* Bitmap control */
3756 		*pos++ = n1 | aid0;
3757 		/* Part Virt Bitmap */
3758 		skb_put(skb, n2 - n1);
3759 		memcpy(pos, ps->tim + n1, n2 - n1 + 1);
3760 
3761 		tim[1] = n2 - n1 + 4;
3762 	} else {
3763 		*pos++ = aid0; /* Bitmap control */
3764 		*pos++ = 0; /* Part Virt Bitmap */
3765 	}
3766 }
3767 
3768 static int ieee80211_beacon_add_tim(struct ieee80211_sub_if_data *sdata,
3769 				    struct ps_data *ps, struct sk_buff *skb,
3770 				    bool is_template)
3771 {
3772 	struct ieee80211_local *local = sdata->local;
3773 
3774 	/*
3775 	 * Not very nice, but we want to allow the driver to call
3776 	 * ieee80211_beacon_get() as a response to the set_tim()
3777 	 * callback. That, however, is already invoked under the
3778 	 * sta_lock to guarantee consistent and race-free update
3779 	 * of the tim bitmap in mac80211 and the driver.
3780 	 */
3781 	if (local->tim_in_locked_section) {
3782 		__ieee80211_beacon_add_tim(sdata, ps, skb, is_template);
3783 	} else {
3784 		spin_lock_bh(&local->tim_lock);
3785 		__ieee80211_beacon_add_tim(sdata, ps, skb, is_template);
3786 		spin_unlock_bh(&local->tim_lock);
3787 	}
3788 
3789 	return 0;
3790 }
3791 
3792 static void ieee80211_set_csa(struct ieee80211_sub_if_data *sdata,
3793 			      struct beacon_data *beacon)
3794 {
3795 	struct probe_resp *resp;
3796 	u8 *beacon_data;
3797 	size_t beacon_data_len;
3798 	int i;
3799 	u8 count = beacon->csa_current_counter;
3800 
3801 	switch (sdata->vif.type) {
3802 	case NL80211_IFTYPE_AP:
3803 		beacon_data = beacon->tail;
3804 		beacon_data_len = beacon->tail_len;
3805 		break;
3806 	case NL80211_IFTYPE_ADHOC:
3807 		beacon_data = beacon->head;
3808 		beacon_data_len = beacon->head_len;
3809 		break;
3810 	case NL80211_IFTYPE_MESH_POINT:
3811 		beacon_data = beacon->head;
3812 		beacon_data_len = beacon->head_len;
3813 		break;
3814 	default:
3815 		return;
3816 	}
3817 
3818 	rcu_read_lock();
3819 	for (i = 0; i < IEEE80211_MAX_CSA_COUNTERS_NUM; ++i) {
3820 		resp = rcu_dereference(sdata->u.ap.probe_resp);
3821 
3822 		if (beacon->csa_counter_offsets[i]) {
3823 			if (WARN_ON_ONCE(beacon->csa_counter_offsets[i] >=
3824 					 beacon_data_len)) {
3825 				rcu_read_unlock();
3826 				return;
3827 			}
3828 
3829 			beacon_data[beacon->csa_counter_offsets[i]] = count;
3830 		}
3831 
3832 		if (sdata->vif.type == NL80211_IFTYPE_AP && resp)
3833 			resp->data[resp->csa_counter_offsets[i]] = count;
3834 	}
3835 	rcu_read_unlock();
3836 }
3837 
3838 static u8 __ieee80211_csa_update_counter(struct beacon_data *beacon)
3839 {
3840 	beacon->csa_current_counter--;
3841 
3842 	/* the counter should never reach 0 */
3843 	WARN_ON_ONCE(!beacon->csa_current_counter);
3844 
3845 	return beacon->csa_current_counter;
3846 }
3847 
3848 u8 ieee80211_csa_update_counter(struct ieee80211_vif *vif)
3849 {
3850 	struct ieee80211_sub_if_data *sdata = vif_to_sdata(vif);
3851 	struct beacon_data *beacon = NULL;
3852 	u8 count = 0;
3853 
3854 	rcu_read_lock();
3855 
3856 	if (sdata->vif.type == NL80211_IFTYPE_AP)
3857 		beacon = rcu_dereference(sdata->u.ap.beacon);
3858 	else if (sdata->vif.type == NL80211_IFTYPE_ADHOC)
3859 		beacon = rcu_dereference(sdata->u.ibss.presp);
3860 	else if (ieee80211_vif_is_mesh(&sdata->vif))
3861 		beacon = rcu_dereference(sdata->u.mesh.beacon);
3862 
3863 	if (!beacon)
3864 		goto unlock;
3865 
3866 	count = __ieee80211_csa_update_counter(beacon);
3867 
3868 unlock:
3869 	rcu_read_unlock();
3870 	return count;
3871 }
3872 EXPORT_SYMBOL(ieee80211_csa_update_counter);
3873 
3874 bool ieee80211_csa_is_complete(struct ieee80211_vif *vif)
3875 {
3876 	struct ieee80211_sub_if_data *sdata = vif_to_sdata(vif);
3877 	struct beacon_data *beacon = NULL;
3878 	u8 *beacon_data;
3879 	size_t beacon_data_len;
3880 	int ret = false;
3881 
3882 	if (!ieee80211_sdata_running(sdata))
3883 		return false;
3884 
3885 	rcu_read_lock();
3886 	if (vif->type == NL80211_IFTYPE_AP) {
3887 		struct ieee80211_if_ap *ap = &sdata->u.ap;
3888 
3889 		beacon = rcu_dereference(ap->beacon);
3890 		if (WARN_ON(!beacon || !beacon->tail))
3891 			goto out;
3892 		beacon_data = beacon->tail;
3893 		beacon_data_len = beacon->tail_len;
3894 	} else if (vif->type == NL80211_IFTYPE_ADHOC) {
3895 		struct ieee80211_if_ibss *ifibss = &sdata->u.ibss;
3896 
3897 		beacon = rcu_dereference(ifibss->presp);
3898 		if (!beacon)
3899 			goto out;
3900 
3901 		beacon_data = beacon->head;
3902 		beacon_data_len = beacon->head_len;
3903 	} else if (vif->type == NL80211_IFTYPE_MESH_POINT) {
3904 		struct ieee80211_if_mesh *ifmsh = &sdata->u.mesh;
3905 
3906 		beacon = rcu_dereference(ifmsh->beacon);
3907 		if (!beacon)
3908 			goto out;
3909 
3910 		beacon_data = beacon->head;
3911 		beacon_data_len = beacon->head_len;
3912 	} else {
3913 		WARN_ON(1);
3914 		goto out;
3915 	}
3916 
3917 	if (!beacon->csa_counter_offsets[0])
3918 		goto out;
3919 
3920 	if (WARN_ON_ONCE(beacon->csa_counter_offsets[0] > beacon_data_len))
3921 		goto out;
3922 
3923 	if (beacon_data[beacon->csa_counter_offsets[0]] == 1)
3924 		ret = true;
3925  out:
3926 	rcu_read_unlock();
3927 
3928 	return ret;
3929 }
3930 EXPORT_SYMBOL(ieee80211_csa_is_complete);
3931 
3932 static struct sk_buff *
3933 __ieee80211_beacon_get(struct ieee80211_hw *hw,
3934 		       struct ieee80211_vif *vif,
3935 		       struct ieee80211_mutable_offsets *offs,
3936 		       bool is_template)
3937 {
3938 	struct ieee80211_local *local = hw_to_local(hw);
3939 	struct beacon_data *beacon = NULL;
3940 	struct sk_buff *skb = NULL;
3941 	struct ieee80211_tx_info *info;
3942 	struct ieee80211_sub_if_data *sdata = NULL;
3943 	enum nl80211_band band;
3944 	struct ieee80211_tx_rate_control txrc;
3945 	struct ieee80211_chanctx_conf *chanctx_conf;
3946 	int csa_off_base = 0;
3947 
3948 	rcu_read_lock();
3949 
3950 	sdata = vif_to_sdata(vif);
3951 	chanctx_conf = rcu_dereference(sdata->vif.chanctx_conf);
3952 
3953 	if (!ieee80211_sdata_running(sdata) || !chanctx_conf)
3954 		goto out;
3955 
3956 	if (offs)
3957 		memset(offs, 0, sizeof(*offs));
3958 
3959 	if (sdata->vif.type == NL80211_IFTYPE_AP) {
3960 		struct ieee80211_if_ap *ap = &sdata->u.ap;
3961 
3962 		beacon = rcu_dereference(ap->beacon);
3963 		if (beacon) {
3964 			if (beacon->csa_counter_offsets[0]) {
3965 				if (!is_template)
3966 					__ieee80211_csa_update_counter(beacon);
3967 
3968 				ieee80211_set_csa(sdata, beacon);
3969 			}
3970 
3971 			/*
3972 			 * headroom, head length,
3973 			 * tail length and maximum TIM length
3974 			 */
3975 			skb = dev_alloc_skb(local->tx_headroom +
3976 					    beacon->head_len +
3977 					    beacon->tail_len + 256 +
3978 					    local->hw.extra_beacon_tailroom);
3979 			if (!skb)
3980 				goto out;
3981 
3982 			skb_reserve(skb, local->tx_headroom);
3983 			memcpy(skb_put(skb, beacon->head_len), beacon->head,
3984 			       beacon->head_len);
3985 
3986 			ieee80211_beacon_add_tim(sdata, &ap->ps, skb,
3987 						 is_template);
3988 
3989 			if (offs) {
3990 				offs->tim_offset = beacon->head_len;
3991 				offs->tim_length = skb->len - beacon->head_len;
3992 
3993 				/* for AP the csa offsets are from tail */
3994 				csa_off_base = skb->len;
3995 			}
3996 
3997 			if (beacon->tail)
3998 				memcpy(skb_put(skb, beacon->tail_len),
3999 				       beacon->tail, beacon->tail_len);
4000 		} else
4001 			goto out;
4002 	} else if (sdata->vif.type == NL80211_IFTYPE_ADHOC) {
4003 		struct ieee80211_if_ibss *ifibss = &sdata->u.ibss;
4004 		struct ieee80211_hdr *hdr;
4005 
4006 		beacon = rcu_dereference(ifibss->presp);
4007 		if (!beacon)
4008 			goto out;
4009 
4010 		if (beacon->csa_counter_offsets[0]) {
4011 			if (!is_template)
4012 				__ieee80211_csa_update_counter(beacon);
4013 
4014 			ieee80211_set_csa(sdata, beacon);
4015 		}
4016 
4017 		skb = dev_alloc_skb(local->tx_headroom + beacon->head_len +
4018 				    local->hw.extra_beacon_tailroom);
4019 		if (!skb)
4020 			goto out;
4021 		skb_reserve(skb, local->tx_headroom);
4022 		memcpy(skb_put(skb, beacon->head_len), beacon->head,
4023 		       beacon->head_len);
4024 
4025 		hdr = (struct ieee80211_hdr *) skb->data;
4026 		hdr->frame_control = cpu_to_le16(IEEE80211_FTYPE_MGMT |
4027 						 IEEE80211_STYPE_BEACON);
4028 	} else if (ieee80211_vif_is_mesh(&sdata->vif)) {
4029 		struct ieee80211_if_mesh *ifmsh = &sdata->u.mesh;
4030 
4031 		beacon = rcu_dereference(ifmsh->beacon);
4032 		if (!beacon)
4033 			goto out;
4034 
4035 		if (beacon->csa_counter_offsets[0]) {
4036 			if (!is_template)
4037 				/* TODO: For mesh csa_counter is in TU, so
4038 				 * decrementing it by one isn't correct, but
4039 				 * for now we leave it consistent with overall
4040 				 * mac80211's behavior.
4041 				 */
4042 				__ieee80211_csa_update_counter(beacon);
4043 
4044 			ieee80211_set_csa(sdata, beacon);
4045 		}
4046 
4047 		if (ifmsh->sync_ops)
4048 			ifmsh->sync_ops->adjust_tbtt(sdata, beacon);
4049 
4050 		skb = dev_alloc_skb(local->tx_headroom +
4051 				    beacon->head_len +
4052 				    256 + /* TIM IE */
4053 				    beacon->tail_len +
4054 				    local->hw.extra_beacon_tailroom);
4055 		if (!skb)
4056 			goto out;
4057 		skb_reserve(skb, local->tx_headroom);
4058 		memcpy(skb_put(skb, beacon->head_len), beacon->head,
4059 		       beacon->head_len);
4060 		ieee80211_beacon_add_tim(sdata, &ifmsh->ps, skb, is_template);
4061 
4062 		if (offs) {
4063 			offs->tim_offset = beacon->head_len;
4064 			offs->tim_length = skb->len - beacon->head_len;
4065 		}
4066 
4067 		memcpy(skb_put(skb, beacon->tail_len), beacon->tail,
4068 		       beacon->tail_len);
4069 	} else {
4070 		WARN_ON(1);
4071 		goto out;
4072 	}
4073 
4074 	/* CSA offsets */
4075 	if (offs && beacon) {
4076 		int i;
4077 
4078 		for (i = 0; i < IEEE80211_MAX_CSA_COUNTERS_NUM; i++) {
4079 			u16 csa_off = beacon->csa_counter_offsets[i];
4080 
4081 			if (!csa_off)
4082 				continue;
4083 
4084 			offs->csa_counter_offs[i] = csa_off_base + csa_off;
4085 		}
4086 	}
4087 
4088 	band = chanctx_conf->def.chan->band;
4089 
4090 	info = IEEE80211_SKB_CB(skb);
4091 
4092 	info->flags |= IEEE80211_TX_INTFL_DONT_ENCRYPT;
4093 	info->flags |= IEEE80211_TX_CTL_NO_ACK;
4094 	info->band = band;
4095 
4096 	memset(&txrc, 0, sizeof(txrc));
4097 	txrc.hw = hw;
4098 	txrc.sband = local->hw.wiphy->bands[band];
4099 	txrc.bss_conf = &sdata->vif.bss_conf;
4100 	txrc.skb = skb;
4101 	txrc.reported_rate.idx = -1;
4102 	txrc.rate_idx_mask = sdata->rc_rateidx_mask[band];
4103 	if (txrc.rate_idx_mask == (1 << txrc.sband->n_bitrates) - 1)
4104 		txrc.max_rate_idx = -1;
4105 	else
4106 		txrc.max_rate_idx = fls(txrc.rate_idx_mask) - 1;
4107 	txrc.bss = true;
4108 	rate_control_get_rate(sdata, NULL, &txrc);
4109 
4110 	info->control.vif = vif;
4111 
4112 	info->flags |= IEEE80211_TX_CTL_CLEAR_PS_FILT |
4113 			IEEE80211_TX_CTL_ASSIGN_SEQ |
4114 			IEEE80211_TX_CTL_FIRST_FRAGMENT;
4115  out:
4116 	rcu_read_unlock();
4117 	return skb;
4118 
4119 }
4120 
4121 struct sk_buff *
4122 ieee80211_beacon_get_template(struct ieee80211_hw *hw,
4123 			      struct ieee80211_vif *vif,
4124 			      struct ieee80211_mutable_offsets *offs)
4125 {
4126 	return __ieee80211_beacon_get(hw, vif, offs, true);
4127 }
4128 EXPORT_SYMBOL(ieee80211_beacon_get_template);
4129 
4130 struct sk_buff *ieee80211_beacon_get_tim(struct ieee80211_hw *hw,
4131 					 struct ieee80211_vif *vif,
4132 					 u16 *tim_offset, u16 *tim_length)
4133 {
4134 	struct ieee80211_mutable_offsets offs = {};
4135 	struct sk_buff *bcn = __ieee80211_beacon_get(hw, vif, &offs, false);
4136 	struct sk_buff *copy;
4137 	struct ieee80211_supported_band *sband;
4138 	int shift;
4139 
4140 	if (!bcn)
4141 		return bcn;
4142 
4143 	if (tim_offset)
4144 		*tim_offset = offs.tim_offset;
4145 
4146 	if (tim_length)
4147 		*tim_length = offs.tim_length;
4148 
4149 	if (ieee80211_hw_check(hw, BEACON_TX_STATUS) ||
4150 	    !hw_to_local(hw)->monitors)
4151 		return bcn;
4152 
4153 	/* send a copy to monitor interfaces */
4154 	copy = skb_copy(bcn, GFP_ATOMIC);
4155 	if (!copy)
4156 		return bcn;
4157 
4158 	shift = ieee80211_vif_get_shift(vif);
4159 	sband = hw->wiphy->bands[ieee80211_get_sdata_band(vif_to_sdata(vif))];
4160 	ieee80211_tx_monitor(hw_to_local(hw), copy, sband, 1, shift, false);
4161 
4162 	return bcn;
4163 }
4164 EXPORT_SYMBOL(ieee80211_beacon_get_tim);
4165 
4166 struct sk_buff *ieee80211_proberesp_get(struct ieee80211_hw *hw,
4167 					struct ieee80211_vif *vif)
4168 {
4169 	struct ieee80211_if_ap *ap = NULL;
4170 	struct sk_buff *skb = NULL;
4171 	struct probe_resp *presp = NULL;
4172 	struct ieee80211_hdr *hdr;
4173 	struct ieee80211_sub_if_data *sdata = vif_to_sdata(vif);
4174 
4175 	if (sdata->vif.type != NL80211_IFTYPE_AP)
4176 		return NULL;
4177 
4178 	rcu_read_lock();
4179 
4180 	ap = &sdata->u.ap;
4181 	presp = rcu_dereference(ap->probe_resp);
4182 	if (!presp)
4183 		goto out;
4184 
4185 	skb = dev_alloc_skb(presp->len);
4186 	if (!skb)
4187 		goto out;
4188 
4189 	memcpy(skb_put(skb, presp->len), presp->data, presp->len);
4190 
4191 	hdr = (struct ieee80211_hdr *) skb->data;
4192 	memset(hdr->addr1, 0, sizeof(hdr->addr1));
4193 
4194 out:
4195 	rcu_read_unlock();
4196 	return skb;
4197 }
4198 EXPORT_SYMBOL(ieee80211_proberesp_get);
4199 
4200 struct sk_buff *ieee80211_pspoll_get(struct ieee80211_hw *hw,
4201 				     struct ieee80211_vif *vif)
4202 {
4203 	struct ieee80211_sub_if_data *sdata;
4204 	struct ieee80211_if_managed *ifmgd;
4205 	struct ieee80211_pspoll *pspoll;
4206 	struct ieee80211_local *local;
4207 	struct sk_buff *skb;
4208 
4209 	if (WARN_ON(vif->type != NL80211_IFTYPE_STATION))
4210 		return NULL;
4211 
4212 	sdata = vif_to_sdata(vif);
4213 	ifmgd = &sdata->u.mgd;
4214 	local = sdata->local;
4215 
4216 	skb = dev_alloc_skb(local->hw.extra_tx_headroom + sizeof(*pspoll));
4217 	if (!skb)
4218 		return NULL;
4219 
4220 	skb_reserve(skb, local->hw.extra_tx_headroom);
4221 
4222 	pspoll = (struct ieee80211_pspoll *) skb_put(skb, sizeof(*pspoll));
4223 	memset(pspoll, 0, sizeof(*pspoll));
4224 	pspoll->frame_control = cpu_to_le16(IEEE80211_FTYPE_CTL |
4225 					    IEEE80211_STYPE_PSPOLL);
4226 	pspoll->aid = cpu_to_le16(ifmgd->aid);
4227 
4228 	/* aid in PS-Poll has its two MSBs each set to 1 */
4229 	pspoll->aid |= cpu_to_le16(1 << 15 | 1 << 14);
4230 
4231 	memcpy(pspoll->bssid, ifmgd->bssid, ETH_ALEN);
4232 	memcpy(pspoll->ta, vif->addr, ETH_ALEN);
4233 
4234 	return skb;
4235 }
4236 EXPORT_SYMBOL(ieee80211_pspoll_get);
4237 
4238 struct sk_buff *ieee80211_nullfunc_get(struct ieee80211_hw *hw,
4239 				       struct ieee80211_vif *vif)
4240 {
4241 	struct ieee80211_hdr_3addr *nullfunc;
4242 	struct ieee80211_sub_if_data *sdata;
4243 	struct ieee80211_if_managed *ifmgd;
4244 	struct ieee80211_local *local;
4245 	struct sk_buff *skb;
4246 
4247 	if (WARN_ON(vif->type != NL80211_IFTYPE_STATION))
4248 		return NULL;
4249 
4250 	sdata = vif_to_sdata(vif);
4251 	ifmgd = &sdata->u.mgd;
4252 	local = sdata->local;
4253 
4254 	skb = dev_alloc_skb(local->hw.extra_tx_headroom + sizeof(*nullfunc));
4255 	if (!skb)
4256 		return NULL;
4257 
4258 	skb_reserve(skb, local->hw.extra_tx_headroom);
4259 
4260 	nullfunc = (struct ieee80211_hdr_3addr *) skb_put(skb,
4261 							  sizeof(*nullfunc));
4262 	memset(nullfunc, 0, sizeof(*nullfunc));
4263 	nullfunc->frame_control = cpu_to_le16(IEEE80211_FTYPE_DATA |
4264 					      IEEE80211_STYPE_NULLFUNC |
4265 					      IEEE80211_FCTL_TODS);
4266 	memcpy(nullfunc->addr1, ifmgd->bssid, ETH_ALEN);
4267 	memcpy(nullfunc->addr2, vif->addr, ETH_ALEN);
4268 	memcpy(nullfunc->addr3, ifmgd->bssid, ETH_ALEN);
4269 
4270 	return skb;
4271 }
4272 EXPORT_SYMBOL(ieee80211_nullfunc_get);
4273 
4274 struct sk_buff *ieee80211_probereq_get(struct ieee80211_hw *hw,
4275 				       const u8 *src_addr,
4276 				       const u8 *ssid, size_t ssid_len,
4277 				       size_t tailroom)
4278 {
4279 	struct ieee80211_local *local = hw_to_local(hw);
4280 	struct ieee80211_hdr_3addr *hdr;
4281 	struct sk_buff *skb;
4282 	size_t ie_ssid_len;
4283 	u8 *pos;
4284 
4285 	ie_ssid_len = 2 + ssid_len;
4286 
4287 	skb = dev_alloc_skb(local->hw.extra_tx_headroom + sizeof(*hdr) +
4288 			    ie_ssid_len + tailroom);
4289 	if (!skb)
4290 		return NULL;
4291 
4292 	skb_reserve(skb, local->hw.extra_tx_headroom);
4293 
4294 	hdr = (struct ieee80211_hdr_3addr *) skb_put(skb, sizeof(*hdr));
4295 	memset(hdr, 0, sizeof(*hdr));
4296 	hdr->frame_control = cpu_to_le16(IEEE80211_FTYPE_MGMT |
4297 					 IEEE80211_STYPE_PROBE_REQ);
4298 	eth_broadcast_addr(hdr->addr1);
4299 	memcpy(hdr->addr2, src_addr, ETH_ALEN);
4300 	eth_broadcast_addr(hdr->addr3);
4301 
4302 	pos = skb_put(skb, ie_ssid_len);
4303 	*pos++ = WLAN_EID_SSID;
4304 	*pos++ = ssid_len;
4305 	if (ssid_len)
4306 		memcpy(pos, ssid, ssid_len);
4307 	pos += ssid_len;
4308 
4309 	return skb;
4310 }
4311 EXPORT_SYMBOL(ieee80211_probereq_get);
4312 
4313 void ieee80211_rts_get(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
4314 		       const void *frame, size_t frame_len,
4315 		       const struct ieee80211_tx_info *frame_txctl,
4316 		       struct ieee80211_rts *rts)
4317 {
4318 	const struct ieee80211_hdr *hdr = frame;
4319 
4320 	rts->frame_control =
4321 	    cpu_to_le16(IEEE80211_FTYPE_CTL | IEEE80211_STYPE_RTS);
4322 	rts->duration = ieee80211_rts_duration(hw, vif, frame_len,
4323 					       frame_txctl);
4324 	memcpy(rts->ra, hdr->addr1, sizeof(rts->ra));
4325 	memcpy(rts->ta, hdr->addr2, sizeof(rts->ta));
4326 }
4327 EXPORT_SYMBOL(ieee80211_rts_get);
4328 
4329 void ieee80211_ctstoself_get(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
4330 			     const void *frame, size_t frame_len,
4331 			     const struct ieee80211_tx_info *frame_txctl,
4332 			     struct ieee80211_cts *cts)
4333 {
4334 	const struct ieee80211_hdr *hdr = frame;
4335 
4336 	cts->frame_control =
4337 	    cpu_to_le16(IEEE80211_FTYPE_CTL | IEEE80211_STYPE_CTS);
4338 	cts->duration = ieee80211_ctstoself_duration(hw, vif,
4339 						     frame_len, frame_txctl);
4340 	memcpy(cts->ra, hdr->addr1, sizeof(cts->ra));
4341 }
4342 EXPORT_SYMBOL(ieee80211_ctstoself_get);
4343 
4344 struct sk_buff *
4345 ieee80211_get_buffered_bc(struct ieee80211_hw *hw,
4346 			  struct ieee80211_vif *vif)
4347 {
4348 	struct ieee80211_local *local = hw_to_local(hw);
4349 	struct sk_buff *skb = NULL;
4350 	struct ieee80211_tx_data tx;
4351 	struct ieee80211_sub_if_data *sdata;
4352 	struct ps_data *ps;
4353 	struct ieee80211_tx_info *info;
4354 	struct ieee80211_chanctx_conf *chanctx_conf;
4355 
4356 	sdata = vif_to_sdata(vif);
4357 
4358 	rcu_read_lock();
4359 	chanctx_conf = rcu_dereference(sdata->vif.chanctx_conf);
4360 
4361 	if (!chanctx_conf)
4362 		goto out;
4363 
4364 	if (sdata->vif.type == NL80211_IFTYPE_AP) {
4365 		struct beacon_data *beacon =
4366 				rcu_dereference(sdata->u.ap.beacon);
4367 
4368 		if (!beacon || !beacon->head)
4369 			goto out;
4370 
4371 		ps = &sdata->u.ap.ps;
4372 	} else if (ieee80211_vif_is_mesh(&sdata->vif)) {
4373 		ps = &sdata->u.mesh.ps;
4374 	} else {
4375 		goto out;
4376 	}
4377 
4378 	if (ps->dtim_count != 0 || !ps->dtim_bc_mc)
4379 		goto out; /* send buffered bc/mc only after DTIM beacon */
4380 
4381 	while (1) {
4382 		skb = skb_dequeue(&ps->bc_buf);
4383 		if (!skb)
4384 			goto out;
4385 		local->total_ps_buffered--;
4386 
4387 		if (!skb_queue_empty(&ps->bc_buf) && skb->len >= 2) {
4388 			struct ieee80211_hdr *hdr =
4389 				(struct ieee80211_hdr *) skb->data;
4390 			/* more buffered multicast/broadcast frames ==> set
4391 			 * MoreData flag in IEEE 802.11 header to inform PS
4392 			 * STAs */
4393 			hdr->frame_control |=
4394 				cpu_to_le16(IEEE80211_FCTL_MOREDATA);
4395 		}
4396 
4397 		if (sdata->vif.type == NL80211_IFTYPE_AP)
4398 			sdata = IEEE80211_DEV_TO_SUB_IF(skb->dev);
4399 		if (!ieee80211_tx_prepare(sdata, &tx, NULL, skb))
4400 			break;
4401 		ieee80211_free_txskb(hw, skb);
4402 	}
4403 
4404 	info = IEEE80211_SKB_CB(skb);
4405 
4406 	tx.flags |= IEEE80211_TX_PS_BUFFERED;
4407 	info->band = chanctx_conf->def.chan->band;
4408 
4409 	if (invoke_tx_handlers(&tx))
4410 		skb = NULL;
4411  out:
4412 	rcu_read_unlock();
4413 
4414 	return skb;
4415 }
4416 EXPORT_SYMBOL(ieee80211_get_buffered_bc);
4417 
4418 int ieee80211_reserve_tid(struct ieee80211_sta *pubsta, u8 tid)
4419 {
4420 	struct sta_info *sta = container_of(pubsta, struct sta_info, sta);
4421 	struct ieee80211_sub_if_data *sdata = sta->sdata;
4422 	struct ieee80211_local *local = sdata->local;
4423 	int ret;
4424 	u32 queues;
4425 
4426 	lockdep_assert_held(&local->sta_mtx);
4427 
4428 	/* only some cases are supported right now */
4429 	switch (sdata->vif.type) {
4430 	case NL80211_IFTYPE_STATION:
4431 	case NL80211_IFTYPE_AP:
4432 	case NL80211_IFTYPE_AP_VLAN:
4433 		break;
4434 	default:
4435 		WARN_ON(1);
4436 		return -EINVAL;
4437 	}
4438 
4439 	if (WARN_ON(tid >= IEEE80211_NUM_UPS))
4440 		return -EINVAL;
4441 
4442 	if (sta->reserved_tid == tid) {
4443 		ret = 0;
4444 		goto out;
4445 	}
4446 
4447 	if (sta->reserved_tid != IEEE80211_TID_UNRESERVED) {
4448 		sdata_err(sdata, "TID reservation already active\n");
4449 		ret = -EALREADY;
4450 		goto out;
4451 	}
4452 
4453 	ieee80211_stop_vif_queues(sdata->local, sdata,
4454 				  IEEE80211_QUEUE_STOP_REASON_RESERVE_TID);
4455 
4456 	synchronize_net();
4457 
4458 	/* Tear down BA sessions so we stop aggregating on this TID */
4459 	if (ieee80211_hw_check(&local->hw, AMPDU_AGGREGATION)) {
4460 		set_sta_flag(sta, WLAN_STA_BLOCK_BA);
4461 		__ieee80211_stop_tx_ba_session(sta, tid,
4462 					       AGG_STOP_LOCAL_REQUEST);
4463 	}
4464 
4465 	queues = BIT(sdata->vif.hw_queue[ieee802_1d_to_ac[tid]]);
4466 	__ieee80211_flush_queues(local, sdata, queues, false);
4467 
4468 	sta->reserved_tid = tid;
4469 
4470 	ieee80211_wake_vif_queues(local, sdata,
4471 				  IEEE80211_QUEUE_STOP_REASON_RESERVE_TID);
4472 
4473 	if (ieee80211_hw_check(&local->hw, AMPDU_AGGREGATION))
4474 		clear_sta_flag(sta, WLAN_STA_BLOCK_BA);
4475 
4476 	ret = 0;
4477  out:
4478 	return ret;
4479 }
4480 EXPORT_SYMBOL(ieee80211_reserve_tid);
4481 
4482 void ieee80211_unreserve_tid(struct ieee80211_sta *pubsta, u8 tid)
4483 {
4484 	struct sta_info *sta = container_of(pubsta, struct sta_info, sta);
4485 	struct ieee80211_sub_if_data *sdata = sta->sdata;
4486 
4487 	lockdep_assert_held(&sdata->local->sta_mtx);
4488 
4489 	/* only some cases are supported right now */
4490 	switch (sdata->vif.type) {
4491 	case NL80211_IFTYPE_STATION:
4492 	case NL80211_IFTYPE_AP:
4493 	case NL80211_IFTYPE_AP_VLAN:
4494 		break;
4495 	default:
4496 		WARN_ON(1);
4497 		return;
4498 	}
4499 
4500 	if (tid != sta->reserved_tid) {
4501 		sdata_err(sdata, "TID to unreserve (%d) isn't reserved\n", tid);
4502 		return;
4503 	}
4504 
4505 	sta->reserved_tid = IEEE80211_TID_UNRESERVED;
4506 }
4507 EXPORT_SYMBOL(ieee80211_unreserve_tid);
4508 
4509 void __ieee80211_tx_skb_tid_band(struct ieee80211_sub_if_data *sdata,
4510 				 struct sk_buff *skb, int tid,
4511 				 enum nl80211_band band)
4512 {
4513 	int ac = ieee802_1d_to_ac[tid & 7];
4514 
4515 	skb_reset_mac_header(skb);
4516 	skb_set_queue_mapping(skb, ac);
4517 	skb->priority = tid;
4518 
4519 	skb->dev = sdata->dev;
4520 
4521 	/*
4522 	 * The other path calling ieee80211_xmit is from the tasklet,
4523 	 * and while we can handle concurrent transmissions locking
4524 	 * requirements are that we do not come into tx with bhs on.
4525 	 */
4526 	local_bh_disable();
4527 	IEEE80211_SKB_CB(skb)->band = band;
4528 	ieee80211_xmit(sdata, NULL, skb);
4529 	local_bh_enable();
4530 }
4531