rx.c 118 KB
Newer Older
1 2 3 4
/*
 * Copyright 2002-2005, Instant802 Networks, Inc.
 * Copyright 2005-2006, Devicescape Software, Inc.
 * Copyright 2006-2007	Jiri Benc <jbenc@suse.cz>
5
 * Copyright 2007-2010	Johannes Berg <johannes@sipsolutions.net>
6
 * Copyright 2013-2014  Intel Mobile Communications GmbH
7
 * Copyright(c) 2015 - 2016 Intel Deutschland GmbH
8 9 10 11 12 13
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 */

14
#include <linux/jiffies.h>
15
#include <linux/slab.h>
16 17 18 19
#include <linux/kernel.h>
#include <linux/skbuff.h>
#include <linux/netdevice.h>
#include <linux/etherdevice.h>
20
#include <linux/rcupdate.h>
21
#include <linux/export.h>
22
#include <linux/bitops.h>
23 24
#include <net/mac80211.h>
#include <net/ieee80211_radiotap.h>
25
#include <asm/unaligned.h>
26 27

#include "ieee80211_i.h"
28
#include "driver-ops.h"
Johannes Berg's avatar
Johannes Berg committed
29
#include "led.h"
30
#include "mesh.h"
31 32 33 34
#include "wep.h"
#include "wpa.h"
#include "tkip.h"
#include "wme.h"
35
#include "rate.h"
36

37 38 39 40 41 42 43 44 45 46
static inline void ieee80211_rx_stats(struct net_device *dev, u32 len)
{
	struct pcpu_sw_netstats *tstats = this_cpu_ptr(dev->tstats);

	u64_stats_update_begin(&tstats->syncp);
	tstats->rx_packets++;
	tstats->rx_bytes += len;
	u64_stats_update_end(&tstats->syncp);
}

47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91
static u8 *ieee80211_get_bssid(struct ieee80211_hdr *hdr, size_t len,
			       enum nl80211_iftype type)
{
	__le16 fc = hdr->frame_control;

	if (ieee80211_is_data(fc)) {
		if (len < 24) /* drop incorrect hdr len (data) */
			return NULL;

		if (ieee80211_has_a4(fc))
			return NULL;
		if (ieee80211_has_tods(fc))
			return hdr->addr1;
		if (ieee80211_has_fromds(fc))
			return hdr->addr2;

		return hdr->addr3;
	}

	if (ieee80211_is_mgmt(fc)) {
		if (len < 24) /* drop incorrect hdr len (mgmt) */
			return NULL;
		return hdr->addr3;
	}

	if (ieee80211_is_ctl(fc)) {
		if (ieee80211_is_pspoll(fc))
			return hdr->addr1;

		if (ieee80211_is_back_req(fc)) {
			switch (type) {
			case NL80211_IFTYPE_STATION:
				return hdr->addr2;
			case NL80211_IFTYPE_AP:
			case NL80211_IFTYPE_AP_VLAN:
				return hdr->addr1;
			default:
				break; /* fall through to the return */
			}
		}
	}

	return NULL;
}

92 93 94 95 96 97 98
/*
 * monitor mode reception
 *
 * This function cleans up the SKB, i.e. it removes all the stuff
 * only useful for monitoring.
 */
static struct sk_buff *remove_monitor_info(struct ieee80211_local *local,
99 100
					   struct sk_buff *skb,
					   unsigned int rtap_vendor_space)
101
{
102
	if (ieee80211_hw_check(&local->hw, RX_INCLUDES_FCS)) {
103
		if (likely(skb->len > FCS_LEN))
Zhu Yi's avatar
Zhu Yi committed
104
			__pskb_trim(skb, skb->len - FCS_LEN);
105 106 107 108
		else {
			/* driver bug */
			WARN_ON(1);
			dev_kfree_skb(skb);
109
			return NULL;
110 111 112
		}
	}

113 114
	__pskb_pull(skb, rtap_vendor_space);

115 116 117
	return skb;
}

118 119
static inline bool should_drop_frame(struct sk_buff *skb, int present_fcs_len,
				     unsigned int rtap_vendor_space)
120
{
121
	struct ieee80211_rx_status *status = IEEE80211_SKB_RXCB(skb);
122 123 124
	struct ieee80211_hdr *hdr;

	hdr = (void *)(skb->data + rtap_vendor_space);
125

126
	if (status->flag & (RX_FLAG_FAILED_FCS_CRC |
127 128
			    RX_FLAG_FAILED_PLCP_CRC |
			    RX_FLAG_ONLY_MONITOR))
129 130
		return true;

131
	if (unlikely(skb->len < 16 + present_fcs_len + rtap_vendor_space))
132 133
		return true;

134 135 136
	if (ieee80211_is_ctl(hdr->frame_control) &&
	    !ieee80211_is_pspoll(hdr->frame_control) &&
	    !ieee80211_is_back_req(hdr->frame_control))
137 138 139
		return true;

	return false;
140 141
}

142
static int
143 144 145
ieee80211_rx_radiotap_hdrlen(struct ieee80211_local *local,
			     struct ieee80211_rx_status *status,
			     struct sk_buff *skb)
146 147 148 149
{
	int len;

	/* always present fields */
150
	len = sizeof(struct ieee80211_radiotap_header) + 8;
151

152 153 154
	/* allocate extra bitmaps */
	if (status->chains)
		len += 4 * hweight8(status->chains);
155 156 157

	if (ieee80211_have_rx_timestamp(status)) {
		len = ALIGN(len, 8);
158
		len += 8;
159
	}
160
	if (ieee80211_hw_check(&local->hw, SIGNAL_DBM))
161 162
		len += 1;

163 164 165 166
	/* antenna field, if we don't have per-chain info */
	if (!status->chains)
		len += 1;

167 168
	/* padding for RX_FLAGS if necessary */
	len = ALIGN(len, 2);
169

170 171 172
	if (status->flag & RX_FLAG_HT) /* HT info */
		len += 3;

173
	if (status->flag & RX_FLAG_AMPDU_DETAILS) {
174
		len = ALIGN(len, 4);
175 176 177
		len += 8;
	}

178 179 180 181 182
	if (status->flag & RX_FLAG_VHT) {
		len = ALIGN(len, 2);
		len += 12;
	}

183 184 185 186 187
	if (local->hw.radiotap_timestamp.units_pos >= 0) {
		len = ALIGN(len, 8);
		len += 12;
	}

188 189 190 191 192
	if (status->chains) {
		/* antenna and antenna signal fields */
		len += 2 * hweight8(status->chains);
	}

193 194 195 196 197 198 199 200 201 202 203 204 205 206 207
	if (status->flag & RX_FLAG_RADIOTAP_VENDOR_DATA) {
		struct ieee80211_vendor_radiotap *rtap = (void *)skb->data;

		/* vendor presence bitmap */
		len += 4;
		/* alignment for fixed 6-byte vendor data header */
		len = ALIGN(len, 2);
		/* vendor data header */
		len += 6;
		if (WARN_ON(rtap->align == 0))
			rtap->align = 1;
		len = ALIGN(len, rtap->align);
		len += rtap->len + rtap->pad;
	}

208 209 210
	return len;
}

211
/*
212 213 214 215 216 217 218 219
 * ieee80211_add_rx_radiotap_header - add radiotap header
 *
 * add a radiotap header containing all the fields which the hardware provided.
 */
static void
ieee80211_add_rx_radiotap_header(struct ieee80211_local *local,
				 struct sk_buff *skb,
				 struct ieee80211_rate *rate,
220
				 int rtap_len, bool has_fcs)
221
{
222
	struct ieee80211_rx_status *status = IEEE80211_SKB_RXCB(skb);
223 224
	struct ieee80211_radiotap_header *rthdr;
	unsigned char *pos;
225 226
	__le32 *it_present;
	u32 it_present_val;
227
	u16 rx_flags = 0;
228
	u16 channel_flags = 0;
229 230
	int mpdulen, chain;
	unsigned long chains = status->chains;
231 232 233 234 235 236 237
	struct ieee80211_vendor_radiotap rtap = {};

	if (status->flag & RX_FLAG_RADIOTAP_VENDOR_DATA) {
		rtap = *(struct ieee80211_vendor_radiotap *)skb->data;
		/* rtap.len and rtap.pad are undone immediately */
		skb_pull(skb, sizeof(rtap) + rtap.len + rtap.pad);
	}
238 239

	mpdulen = skb->len;
240
	if (!(has_fcs && ieee80211_hw_check(&local->hw, RX_INCLUDES_FCS)))
241
		mpdulen += FCS_LEN;
242 243

	rthdr = (struct ieee80211_radiotap_header *)skb_push(skb, rtap_len);
244
	memset(rthdr, 0, rtap_len - rtap.len - rtap.pad);
245
	it_present = &rthdr->it_present;
246 247

	/* radiotap header, set always present flags */
248
	rthdr->it_len = cpu_to_le16(rtap_len);
249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264
	it_present_val = BIT(IEEE80211_RADIOTAP_FLAGS) |
			 BIT(IEEE80211_RADIOTAP_CHANNEL) |
			 BIT(IEEE80211_RADIOTAP_RX_FLAGS);

	if (!status->chains)
		it_present_val |= BIT(IEEE80211_RADIOTAP_ANTENNA);

	for_each_set_bit(chain, &chains, IEEE80211_MAX_CHAINS) {
		it_present_val |=
			BIT(IEEE80211_RADIOTAP_EXT) |
			BIT(IEEE80211_RADIOTAP_RADIOTAP_NAMESPACE);
		put_unaligned_le32(it_present_val, it_present);
		it_present++;
		it_present_val = BIT(IEEE80211_RADIOTAP_ANTENNA) |
				 BIT(IEEE80211_RADIOTAP_DBM_ANTSIGNAL);
	}
265

266 267 268 269 270 271 272 273
	if (status->flag & RX_FLAG_RADIOTAP_VENDOR_DATA) {
		it_present_val |= BIT(IEEE80211_RADIOTAP_VENDOR_NAMESPACE) |
				  BIT(IEEE80211_RADIOTAP_EXT);
		put_unaligned_le32(it_present_val, it_present);
		it_present++;
		it_present_val = rtap.present;
	}

274 275 276 277
	put_unaligned_le32(it_present_val, it_present);

	pos = (void *)(it_present + 1);

278 279 280
	/* the order of the following fields is important */

	/* IEEE80211_RADIOTAP_TSFT */
281
	if (ieee80211_have_rx_timestamp(status)) {
282 283 284
		/* padding */
		while ((pos - (u8 *)rthdr) & 7)
			*pos++ = 0;
285 286 287 288
		put_unaligned_le64(
			ieee80211_calculate_rx_timestamp(local, status,
							 mpdulen, 0),
			pos);
289
		rthdr->it_present |= cpu_to_le32(1 << IEEE80211_RADIOTAP_TSFT);
290 291 292 293
		pos += 8;
	}

	/* IEEE80211_RADIOTAP_FLAGS */
294
	if (has_fcs && ieee80211_hw_check(&local->hw, RX_INCLUDES_FCS))
295
		*pos |= IEEE80211_RADIOTAP_F_FCS;
Johannes Berg's avatar
Johannes Berg committed
296 297
	if (status->flag & (RX_FLAG_FAILED_FCS_CRC | RX_FLAG_FAILED_PLCP_CRC))
		*pos |= IEEE80211_RADIOTAP_F_BADFCS;
298 299
	if (status->flag & RX_FLAG_SHORTPRE)
		*pos |= IEEE80211_RADIOTAP_F_SHORTPRE;
300 301 302
	pos++;

	/* IEEE80211_RADIOTAP_RATE */
303
	if (!rate || status->flag & (RX_FLAG_HT | RX_FLAG_VHT)) {
304
		/*
305
		 * Without rate information don't add it. If we have,
306
		 * MCS information is a separate field in radiotap,
307 308
		 * added below. The byte here is needed as padding
		 * for the channel though, so initialise it to 0.
309 310
		 */
		*pos = 0;
311
	} else {
312
		int shift = 0;
313
		rthdr->it_present |= cpu_to_le32(1 << IEEE80211_RADIOTAP_RATE);
314 315 316 317 318
		if (status->flag & RX_FLAG_10MHZ)
			shift = 1;
		else if (status->flag & RX_FLAG_5MHZ)
			shift = 2;
		*pos = DIV_ROUND_UP(rate->bitrate, 5 * (1 << shift));
319
	}
320 321 322
	pos++;

	/* IEEE80211_RADIOTAP_CHANNEL */
323
	put_unaligned_le16(status->freq, pos);
324
	pos += 2;
325 326 327 328 329
	if (status->flag & RX_FLAG_10MHZ)
		channel_flags |= IEEE80211_CHAN_HALF;
	else if (status->flag & RX_FLAG_5MHZ)
		channel_flags |= IEEE80211_CHAN_QUARTER;

330
	if (status->band == NL80211_BAND_5GHZ)
331
		channel_flags |= IEEE80211_CHAN_OFDM | IEEE80211_CHAN_5GHZ;
332
	else if (status->flag & (RX_FLAG_HT | RX_FLAG_VHT))
333
		channel_flags |= IEEE80211_CHAN_DYN | IEEE80211_CHAN_2GHZ;
334
	else if (rate && rate->flags & IEEE80211_RATE_ERP_G)
335
		channel_flags |= IEEE80211_CHAN_OFDM | IEEE80211_CHAN_2GHZ;
336
	else if (rate)
337
		channel_flags |= IEEE80211_CHAN_CCK | IEEE80211_CHAN_2GHZ;
338
	else
339 340
		channel_flags |= IEEE80211_CHAN_2GHZ;
	put_unaligned_le16(channel_flags, pos);
341 342 343
	pos += 2;

	/* IEEE80211_RADIOTAP_DBM_ANTSIGNAL */
344
	if (ieee80211_hw_check(&local->hw, SIGNAL_DBM) &&
345
	    !(status->flag & RX_FLAG_NO_SIGNAL_VAL)) {
346 347 348 349 350 351 352 353
		*pos = status->signal;
		rthdr->it_present |=
			cpu_to_le32(1 << IEEE80211_RADIOTAP_DBM_ANTSIGNAL);
		pos++;
	}

	/* IEEE80211_RADIOTAP_LOCK_QUALITY is missing */

354 355 356 357 358
	if (!status->chains) {
		/* IEEE80211_RADIOTAP_ANTENNA */
		*pos = status->antenna;
		pos++;
	}
359 360 361 362 363

	/* IEEE80211_RADIOTAP_DB_ANTNOISE is not used */

	/* IEEE80211_RADIOTAP_RX_FLAGS */
	/* ensure 2 byte alignment for the 2 byte field as required */
364
	if ((pos - (u8 *)rthdr) & 1)
365
		*pos++ = 0;
Johannes Berg's avatar
Johannes Berg committed
366
	if (status->flag & RX_FLAG_FAILED_PLCP_CRC)
367 368
		rx_flags |= IEEE80211_RADIOTAP_F_RX_BADPLCP;
	put_unaligned_le16(rx_flags, pos);
369
	pos += 2;
370 371

	if (status->flag & RX_FLAG_HT) {
372 373
		unsigned int stbc;

374
		rthdr->it_present |= cpu_to_le32(1 << IEEE80211_RADIOTAP_MCS);
375
		*pos++ = local->hw.radiotap_mcs_details;
376 377 378 379 380
		*pos = 0;
		if (status->flag & RX_FLAG_SHORT_GI)
			*pos |= IEEE80211_RADIOTAP_MCS_SGI;
		if (status->flag & RX_FLAG_40MHZ)
			*pos |= IEEE80211_RADIOTAP_MCS_BW_40;
381 382
		if (status->flag & RX_FLAG_HT_GF)
			*pos |= IEEE80211_RADIOTAP_MCS_FMT_GF;
383 384
		if (status->flag & RX_FLAG_LDPC)
			*pos |= IEEE80211_RADIOTAP_MCS_FEC_LDPC;
385 386
		stbc = (status->flag & RX_FLAG_STBC_MASK) >> RX_FLAG_STBC_SHIFT;
		*pos |= stbc << IEEE80211_RADIOTAP_MCS_STBC_SHIFT;
387 388 389
		pos++;
		*pos++ = status->rate_idx;
	}
390 391 392 393 394 395 396 397 398 399 400 401 402 403 404 405 406 407 408 409 410 411 412 413 414 415 416

	if (status->flag & RX_FLAG_AMPDU_DETAILS) {
		u16 flags = 0;

		/* ensure 4 byte alignment */
		while ((pos - (u8 *)rthdr) & 3)
			pos++;
		rthdr->it_present |=
			cpu_to_le32(1 << IEEE80211_RADIOTAP_AMPDU_STATUS);
		put_unaligned_le32(status->ampdu_reference, pos);
		pos += 4;
		if (status->flag & RX_FLAG_AMPDU_LAST_KNOWN)
			flags |= IEEE80211_RADIOTAP_AMPDU_LAST_KNOWN;
		if (status->flag & RX_FLAG_AMPDU_IS_LAST)
			flags |= IEEE80211_RADIOTAP_AMPDU_IS_LAST;
		if (status->flag & RX_FLAG_AMPDU_DELIM_CRC_ERROR)
			flags |= IEEE80211_RADIOTAP_AMPDU_DELIM_CRC_ERR;
		if (status->flag & RX_FLAG_AMPDU_DELIM_CRC_KNOWN)
			flags |= IEEE80211_RADIOTAP_AMPDU_DELIM_CRC_KNOWN;
		put_unaligned_le16(flags, pos);
		pos += 2;
		if (status->flag & RX_FLAG_AMPDU_DELIM_CRC_KNOWN)
			*pos++ = status->ampdu_delimiter_crc;
		else
			*pos++ = 0;
		*pos++ = 0;
	}
417

418 419 420 421 422 423 424 425 426
	if (status->flag & RX_FLAG_VHT) {
		u16 known = local->hw.radiotap_vht_details;

		rthdr->it_present |= cpu_to_le32(1 << IEEE80211_RADIOTAP_VHT);
		put_unaligned_le16(known, pos);
		pos += 2;
		/* flags */
		if (status->flag & RX_FLAG_SHORT_GI)
			*pos |= IEEE80211_RADIOTAP_VHT_FLAG_SGI;
427 428 429
		/* in VHT, STBC is binary */
		if (status->flag & RX_FLAG_STBC_MASK)
			*pos |= IEEE80211_RADIOTAP_VHT_FLAG_STBC;
430 431
		if (status->vht_flag & RX_VHT_FLAG_BF)
			*pos |= IEEE80211_RADIOTAP_VHT_FLAG_BEAMFORMED;
432 433
		pos++;
		/* bandwidth */
434
		if (status->vht_flag & RX_VHT_FLAG_80MHZ)
435
			*pos++ = 4;
436
		else if (status->vht_flag & RX_VHT_FLAG_160MHZ)
437 438 439 440 441 442 443 444 445
			*pos++ = 11;
		else if (status->flag & RX_FLAG_40MHZ)
			*pos++ = 1;
		else /* 20 MHz */
			*pos++ = 0;
		/* MCS/NSS */
		*pos = (status->rate_idx << 4) | status->vht_nss;
		pos += 4;
		/* coding field */
446 447
		if (status->flag & RX_FLAG_LDPC)
			*pos |= IEEE80211_RADIOTAP_CODING_LDPC_USER0;
448 449 450 451 452 453 454
		pos++;
		/* group ID */
		pos++;
		/* partial_aid */
		pos += 2;
	}

455 456 457 458 459 460 461 462 463 464 465 466 467 468 469 470 471 472 473 474 475 476 477 478 479
	if (local->hw.radiotap_timestamp.units_pos >= 0) {
		u16 accuracy = 0;
		u8 flags = IEEE80211_RADIOTAP_TIMESTAMP_FLAG_32BIT;

		rthdr->it_present |=
			cpu_to_le32(1 << IEEE80211_RADIOTAP_TIMESTAMP);

		/* ensure 8 byte alignment */
		while ((pos - (u8 *)rthdr) & 7)
			pos++;

		put_unaligned_le64(status->device_timestamp, pos);
		pos += sizeof(u64);

		if (local->hw.radiotap_timestamp.accuracy >= 0) {
			accuracy = local->hw.radiotap_timestamp.accuracy;
			flags |= IEEE80211_RADIOTAP_TIMESTAMP_FLAG_ACCURACY;
		}
		put_unaligned_le16(accuracy, pos);
		pos += sizeof(u16);

		*pos++ = local->hw.radiotap_timestamp.units_pos;
		*pos++ = flags;
	}

480 481 482 483
	for_each_set_bit(chain, &chains, IEEE80211_MAX_CHAINS) {
		*pos++ = status->chain_signal[chain];
		*pos++ = chain;
	}
484 485 486 487 488 489 490 491 492 493 494 495 496 497 498 499

	if (status->flag & RX_FLAG_RADIOTAP_VENDOR_DATA) {
		/* ensure 2 byte alignment for the vendor field as required */
		if ((pos - (u8 *)rthdr) & 1)
			*pos++ = 0;
		*pos++ = rtap.oui[0];
		*pos++ = rtap.oui[1];
		*pos++ = rtap.oui[2];
		*pos++ = rtap.subns;
		put_unaligned_le16(rtap.len, pos);
		pos += 2;
		/* align the actual payload as requested */
		while ((pos - (u8 *)rthdr) & (rtap.align - 1))
			*pos++ = 0;
		/* data (and possible padding) already follows */
	}
500 501
}

502 503 504 505 506 507 508
/*
 * This function copies a received frame to all monitor interfaces and
 * returns a cleaned-up SKB that no longer includes the FCS nor the
 * radiotap header the driver might have added.
 */
static struct sk_buff *
ieee80211_rx_monitor(struct ieee80211_local *local, struct sk_buff *origskb,
509
		     struct ieee80211_rate *rate)
510
{
511
	struct ieee80211_rx_status *status = IEEE80211_SKB_RXCB(origskb);
512
	struct ieee80211_sub_if_data *sdata;
513
	int rt_hdrlen, needed_headroom;
514 515 516
	struct sk_buff *skb, *skb2;
	struct net_device *prev_dev = NULL;
	int present_fcs_len = 0;
517
	unsigned int rtap_vendor_space = 0;
518 519 520
	struct ieee80211_mgmt *mgmt;
	struct ieee80211_sub_if_data *monitor_sdata =
		rcu_dereference(local->monitor_sdata);
521 522 523 524 525 526

	if (unlikely(status->flag & RX_FLAG_RADIOTAP_VENDOR_DATA)) {
		struct ieee80211_vendor_radiotap *rtap = (void *)origskb->data;

		rtap_vendor_space = sizeof(*rtap) + rtap->len + rtap->pad;
	}
527 528 529 530 531 532 533 534 535

	/*
	 * First, we may need to make a copy of the skb because
	 *  (1) we need to modify it for radiotap (if not present), and
	 *  (2) the other RX handlers will modify the skb we got.
	 *
	 * We don't need to, of course, if we aren't going to return
	 * the SKB because it has a bad FCS/PLCP checksum.
	 */
536

537
	if (ieee80211_hw_check(&local->hw, RX_INCLUDES_FCS))
538 539
		present_fcs_len = FCS_LEN;

540 541
	/* ensure hdr->frame_control and vendor radiotap data are in skb head */
	if (!pskb_may_pull(origskb, 2 + rtap_vendor_space)) {
Zhu Yi's avatar
Zhu Yi committed
542 543 544 545
		dev_kfree_skb(origskb);
		return NULL;
	}

546
	if (!local->monitors || (status->flag & RX_FLAG_SKIP_MONITOR)) {
547 548
		if (should_drop_frame(origskb, present_fcs_len,
				      rtap_vendor_space)) {
549 550 551 552
			dev_kfree_skb(origskb);
			return NULL;
		}

553
		return remove_monitor_info(local, origskb, rtap_vendor_space);
554 555
	}

556
	/* room for the radiotap header based on driver features */
557 558
	rt_hdrlen = ieee80211_rx_radiotap_hdrlen(local, status, origskb);
	needed_headroom = rt_hdrlen - rtap_vendor_space;
559

560
	if (should_drop_frame(origskb, present_fcs_len, rtap_vendor_space)) {
561 562 563 564 565 566 567 568 569 570 571 572
		/* only need to expand headroom if necessary */
		skb = origskb;
		origskb = NULL;

		/*
		 * This shouldn't trigger often because most devices have an
		 * RX header they pull before we get here, and that should
		 * be big enough for our radiotap information. We should
		 * probably export the length to drivers so that we can have
		 * them allocate enough headroom to start with.
		 */
		if (skb_headroom(skb) < needed_headroom &&
573
		    pskb_expand_head(skb, needed_headroom, 0, GFP_ATOMIC)) {
574 575 576 577 578 579 580 581 582 583
			dev_kfree_skb(skb);
			return NULL;
		}
	} else {
		/*
		 * Need to make a copy and possibly remove radiotap header
		 * and FCS from the original.
		 */
		skb = skb_copy_expand(origskb, needed_headroom, 0, GFP_ATOMIC);

584 585
		origskb = remove_monitor_info(local, origskb,
					      rtap_vendor_space);
586 587 588 589 590

		if (!skb)
			return origskb;
	}

591
	/* prepend radiotap information */
592
	ieee80211_add_rx_radiotap_header(local, skb, rate, rt_hdrlen, true);
593

594
	skb_reset_mac_header(skb);
595 596 597 598 599
	skb->ip_summed = CHECKSUM_UNNECESSARY;
	skb->pkt_type = PACKET_OTHERHOST;
	skb->protocol = htons(ETH_P_802_2);

	list_for_each_entry_rcu(sdata, &local->interfaces, list) {
600
		if (sdata->vif.type != NL80211_IFTYPE_MONITOR)
601 602
			continue;

603
		if (sdata->u.mntr.flags & MONITOR_FLAG_COOK_FRAMES)
604 605
			continue;

606
		if (!ieee80211_sdata_running(sdata))
607 608
			continue;

609 610 611 612
		if (prev_dev) {
			skb2 = skb_clone(skb, GFP_ATOMIC);
			if (skb2) {
				skb2->dev = prev_dev;
613
				netif_receive_skb(skb2);
614 615 616 617
			}
		}

		prev_dev = sdata->dev;
618
		ieee80211_rx_stats(sdata->dev, skb->len);
619 620
	}

621 622 623 624 625 626 627 628 629 630 631 632 633 634 635 636 637
	mgmt = (void *)skb->data;
	if (monitor_sdata &&
	    skb->len >= IEEE80211_MIN_ACTION_SIZE + 1 + VHT_MUMIMO_GROUPS_DATA_LEN &&
	    ieee80211_is_action(mgmt->frame_control) &&
	    mgmt->u.action.category == WLAN_CATEGORY_VHT &&
	    mgmt->u.action.u.vht_group_notif.action_code == WLAN_VHT_ACTION_GROUPID_MGMT &&
	    is_valid_ether_addr(monitor_sdata->u.mntr.mu_follow_addr) &&
	    ether_addr_equal(mgmt->da, monitor_sdata->u.mntr.mu_follow_addr)) {
		struct sk_buff *mu_skb = skb_copy(skb, GFP_ATOMIC);

		if (mu_skb) {
			mu_skb->pkt_type = IEEE80211_SDATA_QUEUE_TYPE_FRAME;
			skb_queue_tail(&monitor_sdata->skb_queue, mu_skb);
			ieee80211_queue_work(&local->hw, &monitor_sdata->work);
		}
	}

638 639
	if (prev_dev) {
		skb->dev = prev_dev;
640
		netif_receive_skb(skb);
641 642 643 644 645 646
	} else
		dev_kfree_skb(skb);

	return origskb;
}

647
static void ieee80211_parse_qos(struct ieee80211_rx_data *rx)
648
{
649
	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)rx->skb->data;
650
	struct ieee80211_rx_status *status = IEEE80211_SKB_RXCB(rx->skb);
651
	int tid, seqno_idx, security_idx;
652 653

	/* does the frame have a qos control field? */
654 655
	if (ieee80211_is_data_qos(hdr->frame_control)) {
		u8 *qc = ieee80211_get_qos_ctl(hdr);
656
		/* frame has qos control */
657
		tid = *qc & IEEE80211_QOS_CTL_TID_MASK;
658
		if (*qc & IEEE80211_QOS_CTL_A_MSDU_PRESENT)
659
			status->rx_flags |= IEEE80211_RX_AMSDU;
660 661 662

		seqno_idx = tid;
		security_idx = tid;
663
	} else {
664 665 666 667 668 669 670 671 672 673 674
		/*
		 * IEEE 802.11-2007, 7.1.3.4.1 ("Sequence Number field"):
		 *
		 *	Sequence numbers for management frames, QoS data
		 *	frames with a broadcast/multicast address in the
		 *	Address 1 field, and all non-QoS data frames sent
		 *	by QoS STAs are assigned using an additional single
		 *	modulo-4096 counter, [...]
		 *
		 * We also use that counter for non-QoS STAs.
		 */
675
		seqno_idx = IEEE80211_NUM_TIDS;
676 677
		security_idx = 0;
		if (ieee80211_is_mgmt(hdr->frame_control))
678
			security_idx = IEEE80211_NUM_TIDS;
679
		tid = 0;
680
	}
681

682 683
	rx->seqno_idx = seqno_idx;
	rx->security_idx = security_idx;
684 685 686
	/* Set skb->priority to 1d tag if highest order bit of TID is not set.
	 * For now, set skb->priority to 0 for other cases. */
	rx->skb->priority = (tid > 7) ? 0 : tid;
687
}
688

689 690 691 692 693 694 695 696 697 698 699
/**
 * DOC: Packet alignment
 *
 * Drivers always need to pass packets that are aligned to two-byte boundaries
 * to the stack.
 *
 * Additionally, should, if possible, align the payload data in a way that
 * guarantees that the contained IP header is aligned to a four-byte
 * boundary. In the case of regular frames, this simply means aligning the
 * payload to a four-byte boundary (because either the IP header is directly
 * contained, or IV/RFC1042 headers that have a length divisible by four are
700 701 702
 * in front of it).  If the payload data is not properly aligned and the
 * architecture doesn't support efficient unaligned operations, mac80211
 * will align the data.
703 704 705 706 707 708 709
 *
 * With A-MSDU frames, however, the payload data address must yield two modulo
 * four because there are 14-byte 802.3 headers within the A-MSDU frames that
 * push the IP header further back to a multiple of four again. Thankfully, the
 * specs were sane enough this time around to require padding each A-MSDU
 * subframe to a length that is a multiple of four.
 *
Lucas De Marchi's avatar
Lucas De Marchi committed
710
 * Padding like Atheros hardware adds which is between the 802.11 header and
711 712 713 714
 * the payload is not supported, the driver is required to move the 802.11
 * header to be directly in front of the payload in that case.
 */
static void ieee80211_verify_alignment(struct ieee80211_rx_data *rx)
715
{
716
#ifdef CONFIG_MAC80211_VERBOSE_DEBUG
717
	WARN_ON_ONCE((unsigned long)rx->skb->data & 1);
718
#endif
719 720
}

721

722 723
/* rx handlers */

724 725 726 727
static int ieee80211_is_unicast_robust_mgmt_frame(struct sk_buff *skb)
{
	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;

728
	if (is_multicast_ether_addr(hdr->addr1))
729 730
		return 0;

731
	return ieee80211_is_robust_mgmt_frame(skb);
732 733 734 735 736 737 738
}


static int ieee80211_is_multicast_robust_mgmt_frame(struct sk_buff *skb)
{
	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;

739
	if (!is_multicast_ether_addr(hdr->addr1))
740 741
		return 0;

742
	return ieee80211_is_robust_mgmt_frame(skb);
743 744 745 746 747 748 749 750
}


/* Get the BIP key index from MMIE; return -1 if this is not a BIP frame */
static int ieee80211_get_mmie_keyidx(struct sk_buff *skb)
{
	struct ieee80211_mgmt *hdr = (struct ieee80211_mgmt *) skb->data;
	struct ieee80211_mmie *mmie;
751
	struct ieee80211_mmie_16 *mmie16;
752

753
	if (skb->len < 24 + sizeof(*mmie) || !is_multicast_ether_addr(hdr->da))
754 755
		return -1;

756
	if (!ieee80211_is_robust_mgmt_frame(skb))
757 758 759 760
		return -1; /* not a robust management frame */

	mmie = (struct ieee80211_mmie *)
		(skb->data + skb->len - sizeof(*mmie));
761 762 763 764 765 766 767 768 769 770 771 772
	if (mmie->element_id == WLAN_EID_MMIE &&
	    mmie->length == sizeof(*mmie) - 2)
		return le16_to_cpu(mmie->key_id);

	mmie16 = (struct ieee80211_mmie_16 *)
		(skb->data + skb->len - sizeof(*mmie16));
	if (skb->len >= 24 + sizeof(*mmie16) &&
	    mmie16->element_id == WLAN_EID_MMIE &&
	    mmie16->length == sizeof(*mmie16) - 2)
		return le16_to_cpu(mmie16->key_id);

	return -1;
773 774
}

775 776
static int ieee80211_get_cs_keyid(const struct ieee80211_cipher_scheme *cs,
				  struct sk_buff *skb)
777 778 779 780 781 782 783 784 785 786 787 788 789 790 791 792 793 794 795
{
	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
	__le16 fc;
	int hdrlen;
	u8 keyid;

	fc = hdr->frame_control;
	hdrlen = ieee80211_hdrlen(fc);

	if (skb->len < hdrlen + cs->hdr_len)
		return -EINVAL;

	skb_copy_bits(skb, hdrlen + cs->key_idx_off, &keyid, 1);
	keyid &= cs->key_idx_mask;
	keyid >>= cs->key_idx_shift;

	return keyid;
}

796
static ieee80211_rx_result ieee80211_rx_mesh_check(struct ieee80211_rx_data *rx)
797
{
798
	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)rx->skb->data;
799
	char *dev_addr = rx->sdata->vif.addr;
800

801
	if (ieee80211_is_data(hdr->frame_control)) {
802 803
		if (is_multicast_ether_addr(hdr->addr1)) {
			if (ieee80211_has_tods(hdr->frame_control) ||
804
			    !ieee80211_has_fromds(hdr->frame_control))
805
				return RX_DROP_MONITOR;
806
			if (ether_addr_equal(hdr->addr3, dev_addr))
807 808 809 810
				return RX_DROP_MONITOR;
		} else {
			if (!ieee80211_has_a4(hdr->frame_control))
				return RX_DROP_MONITOR;
811
			if (ether_addr_equal(hdr->addr4, dev_addr))
812 813
				return RX_DROP_MONITOR;
		}
814 815 816 817 818 819
	}

	/* If there is not an established peer link and this is not a peer link
	 * establisment frame, beacon or probe, drop the frame.
	 */

820
	if (!rx->sta || sta_plink_state(rx->sta) != NL80211_PLINK_ESTAB) {
821
		struct ieee80211_mgmt *mgmt;
822

823
		if (!ieee80211_is_mgmt(hdr->frame_control))
824 825
			return RX_DROP_MONITOR;

826
		if (ieee80211_is_action(hdr->frame_control)) {
827
			u8 category;
828 829 830 831 832

			/* make sure category field is present */
			if (rx->skb->len < IEEE80211_MIN_ACTION_SIZE)
				return RX_DROP_MONITOR;

833
			mgmt = (struct ieee80211_mgmt *)hdr;
834 835
			category = mgmt->u.action.category;
			if (category != WLAN_CATEGORY_MESH_ACTION &&
836
			    category != WLAN_CATEGORY_SELF_PROTECTED)
837 838 839 840
				return RX_DROP_MONITOR;
			return RX_CONTINUE;
		}

841 842
		if (ieee80211_is_probe_req(hdr->frame_control) ||
		    ieee80211_is_probe_resp(hdr->frame_control) ||
843 844
		    ieee80211_is_beacon(hdr->frame_control) ||
		    ieee80211_is_auth(hdr->frame_control))
845 846 847 848 849
			return RX_CONTINUE;

		return RX_DROP_MONITOR;
	}

Johannes Berg's avatar
Johannes Berg committed
850 851
	return RX_CONTINUE;
}
852

853 854 855 856 857 858 859
static inline bool ieee80211_rx_reorder_ready(struct tid_ampdu_rx *tid_agg_rx,
					      int index)
{
	struct sk_buff_head *frames = &tid_agg_rx->reorder_buf[index];
	struct sk_buff *tail = skb_peek_tail(frames);
	struct ieee80211_rx_status *status;

860 861 862
	if (tid_agg_rx->reorder_buf_filtered & BIT_ULL(index))
		return true;

863 864 865 866 867 868 869 870 871 872
	if (!tail)
		return false;

	status = IEEE80211_SKB_RXCB(tail);
	if (status->flag & RX_FLAG_AMSDU_MORE)
		return false;

	return true;
}

873
static void ieee80211_release_reorder_frame(struct ieee80211_sub_if_data *sdata,
874
					    struct tid_ampdu_rx *tid_agg_rx,
875 876
					    int index,
					    struct sk_buff_head *frames)
877
{
878 879
	struct sk_buff_head *skb_list = &tid_agg_rx->reorder_buf[index];
	struct sk_buff *skb;
880
	struct ieee80211_rx_status *status;
881

882 883
	lockdep_assert_held(&tid_agg_rx->reorder_lock);

884
	if (skb_queue_empty(skb_list))
885 886
		goto no_frame;

887
	if (!ieee80211_rx_reorder_ready(tid_agg_rx, index)) {
888 889 890 891 892
		__skb_queue_purge(skb_list);
		goto no_frame;
	}

	/* release frames from the reorder ring buffer */
893
	tid_agg_rx->stored_mpdu_num--;
894 895 896 897 898
	while ((skb = __skb_dequeue(skb_list))) {
		status = IEEE80211_SKB_RXCB(skb);
		status->rx_flags |= IEEE80211_RX_DEFERRED_RELEASE;
		__skb_queue_tail(frames, skb);
	}
899 900

no_frame:
901
	tid_agg_rx->reorder_buf_filtered &= ~BIT_ULL(index);
902
	tid_agg_rx->head_seq_num = ieee80211_sn_inc(tid_agg_rx->head_seq_num);
903 904
}

905
static void ieee80211_release_reorder_frames(struct ieee80211_sub_if_data *sdata,
906
					     struct tid_ampdu_rx *tid_agg_rx,
907 908
					     u16 head_seq_num,
					     struct sk_buff_head *frames)
909 910 911
{
	int index;

912 913
	lockdep_assert_held(&tid_agg_rx->reorder_lock);

914
	while (ieee80211_sn_less(tid_agg_rx->head_seq_num, head_seq_num)) {
915
		index = tid_agg_rx->head_seq_num % tid_agg_rx->buf_size;
916 917
		ieee80211_release_reorder_frame(sdata, tid_agg_rx, index,
						frames);
918 919 920 921 922 923 924 925 926
	}
}

/*
 * Timeout (in jiffies) for skb's that are waiting in the RX reorder buffer. If
 * the skb was added to the buffer longer than this time ago, the earlier
 * frames that have not yet been received are assumed to be lost and the skb
 * can be released for processing. This may also release other skb's from the
 * reorder buffer if there are no additional gaps between the frames.
927 928
 *
 * Callers must hold tid_agg_rx->reorder_lock.
929 930 931
 */
#define HT_RX_REORDER_BUF_TIMEOUT (HZ / 10)

932
static void ieee80211_sta_reorder_release(struct ieee80211_sub_if_data *sdata,
933 934
					  struct tid_ampdu_rx *tid_agg_rx,
					  struct sk_buff_head *frames)
935
{
936
	int index, i, j;
937

938 939
	lockdep_assert_held(&tid_agg_rx->reorder_lock);

940
	/* release the buffer until next missing frame */
941
	index = tid_agg_rx->head_seq_num % tid_agg_rx->buf_size;
942
	if (!ieee80211_rx_reorder_ready(tid_agg_rx, index) &&
943
	    tid_agg_rx->stored_mpdu_num) {
944 945 946 947 948 949 950
		/*
		 * No buffers ready to be released, but check whether any
		 * frames in the reorder buffer have timed out.
		 */
		int skipped = 1;
		for (j = (index + 1) % tid_agg_rx->buf_size; j != index;
		     j = (j + 1) % tid_agg_rx->buf_size) {
951
			if (!ieee80211_rx_reorder_ready(tid_agg_rx, j)) {
952 953 954
				skipped++;
				continue;
			}
955 956
			if (skipped &&
			    !time_after(jiffies, tid_agg_rx->reorder_time[j] +
957
					HT_RX_REORDER_BUF_TIMEOUT))
958
				goto set_release_timer;
959

960 961 962 963 964
			/* don't leave incomplete A-MSDUs around */
			for (i = (index + 1) % tid_agg_rx->buf_size; i != j;
			     i = (i + 1) % tid_agg_rx->buf_size)
				__skb_queue_purge(&tid_agg_rx->reorder_buf[i]);

Johannes Berg's avatar
Johannes Berg committed
965 966
			ht_dbg_ratelimited(sdata,
					   "release an RX reorder frame due to timeout on earlier frames\n");
967 968
			ieee80211_release_reorder_frame(sdata, tid_agg_rx, j,
							frames);
969 970 971 972 973

			/*
			 * Increment the head seq# also for the skipped slots.
			 */
			tid_agg_rx->head_seq_num =
974 975
				(tid_agg_rx->head_seq_num +
				 skipped) & IEEE80211_SN_MASK;
976 977
			skipped = 0;
		}
978
	} else while (ieee80211_rx_reorder_ready(tid_agg_rx, index)) {
979 980
		ieee80211_release_reorder_frame(sdata, tid_agg_rx, index,
						frames);
981
		index =	tid_agg_rx->head_seq_num % tid_agg_rx->buf_size;
982
	}
983 984

	if (tid_agg_rx->stored_mpdu_num) {
985
		j = index = tid_agg_rx->head_seq_num % tid_agg_rx->buf_size;
986 987 988

		for (; j != (index - 1) % tid_agg_rx->buf_size;
		     j = (j + 1) % tid_agg_rx->buf_size) {
989
			if (ieee80211_rx_reorder_ready(tid_agg_rx, j))
990 991 992 993 994
				break;
		}

 set_release_timer:

995 996 997 998
		if (!tid_agg_rx->removed)
			mod_timer(&tid_agg_rx->reorder_timer,
				  tid_agg_rx->reorder_time[j] + 1 +
				  HT_RX_REORDER_BUF_TIMEOUT);
999 1000 1001
	} else {
		del_timer(&tid_agg_rx->reorder_timer);
	}
1002 1003
}

1004 1005 1006 1007 1008
/*
 * As this function belongs to the RX path it must be under
 * rcu_read_lock protection. It returns false if the frame
 * can be processed immediately, true if it was consumed.
 */
1009
static bool ieee80211_sta_manage_reorder_buf(struct ieee80211_sub_if_data *sdata,
1010
					     struct tid_ampdu_rx *tid_agg_rx,
1011 1012
					     struct sk_buff *skb,
					     struct sk_buff_head *frames)
1013 1014
{
	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
1015
	struct ieee80211_rx_status *status = IEEE80211_SKB_RXCB(skb);
1016 1017 1018 1019
	u16 sc = le16_to_cpu(hdr->seq_ctrl);
	u16 mpdu_seq_num = (sc & IEEE80211_SCTL_SEQ) >> 4;
	u16 head_seq_num, buf_size;
	int index;
1020
	bool ret = true;
1021

1022 1023
	spin_lock(&tid_agg_rx->reorder_lock);

1024 1025 1026 1027 1028 1029 1030 1031 1032 1033
	/*
	 * Offloaded BA sessions have no known starting sequence number so pick
	 * one from first Rxed frame for this tid after BA was started.
	 */
	if (unlikely(tid_agg_rx->auto_seq)) {
		tid_agg_rx->auto_seq = false;
		tid_agg_rx->ssn = mpdu_seq_num;
		tid_agg_rx->head_seq_num = mpdu_seq_num;
	}

1034 1035 1036 1037
	buf_size = tid_agg_rx->buf_size;
	head_seq_num = tid_agg_rx->head_seq_num;

	/* frame with out of date sequence number */
1038
	if (ieee80211_sn_less(mpdu_seq_num, head_seq_num)) {
1039
		dev_kfree_skb(skb);
1040
		goto out;
1041 1042 1043 1044 1045 1046
	}

	/*
	 * If frame the sequence number exceeds our buffering window
	 * size release some previous frames to make room for this one.
	 */
1047 1048 1049
	if (!ieee80211_sn_less(mpdu_seq_num, head_seq_num + buf_size)) {
		head_seq_num = ieee80211_sn_inc(
				ieee80211_sn_sub(mpdu_seq_num, buf_size));
1050
		/* release stored frames up to new head to stack */
1051
		ieee80211_release_reorder_frames(sdata, tid_agg_rx,
1052
						 head_seq_num, frames);
Johannes Berg's avatar