util.c 42.6 KB
Newer Older
1 2 3
/*
 * Wireless utility functions
 *
Johannes Berg's avatar
Johannes Berg committed
4
 * Copyright 2007-2009	Johannes Berg <johannes@sipsolutions.net>
5
 * Copyright 2013-2014  Intel Mobile Communications GmbH
6
 */
7
#include <linux/export.h>
Johannes Berg's avatar
Johannes Berg committed
8
#include <linux/bitops.h>
9
#include <linux/etherdevice.h>
10
#include <linux/slab.h>
Johannes Berg's avatar
Johannes Berg committed
11
#include <net/cfg80211.h>
12
#include <net/ip.h>
13
#include <net/dsfield.h>
14
#include <linux/if_vlan.h>
15
#include <linux/mpls.h>
16
#include "core.h"
17 18
#include "rdev-ops.h"

19

20 21
struct ieee80211_rate *
ieee80211_get_response_rate(struct ieee80211_supported_band *sband,
22
			    u32 basic_rates, int bitrate)
23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38
{
	struct ieee80211_rate *result = &sband->bitrates[0];
	int i;

	for (i = 0; i < sband->n_bitrates; i++) {
		if (!(basic_rates & BIT(i)))
			continue;
		if (sband->bitrates[i].bitrate > bitrate)
			continue;
		result = &sband->bitrates[i];
	}

	return result;
}
EXPORT_SYMBOL(ieee80211_get_response_rate);

39 40
u32 ieee80211_mandatory_rates(struct ieee80211_supported_band *sband,
			      enum nl80211_bss_scan_width scan_width)
41 42 43 44 45 46 47 48 49
{
	struct ieee80211_rate *bitrates;
	u32 mandatory_rates = 0;
	enum ieee80211_rate_flags mandatory_flag;
	int i;

	if (WARN_ON(!sband))
		return 1;

50 51 52 53 54 55 56
	if (sband->band == IEEE80211_BAND_2GHZ) {
		if (scan_width == NL80211_BSS_CHAN_WIDTH_5 ||
		    scan_width == NL80211_BSS_CHAN_WIDTH_10)
			mandatory_flag = IEEE80211_RATE_MANDATORY_G;
		else
			mandatory_flag = IEEE80211_RATE_MANDATORY_B;
	} else {
57
		mandatory_flag = IEEE80211_RATE_MANDATORY_A;
58
	}
59 60 61 62 63 64 65 66 67

	bitrates = sband->bitrates;
	for (i = 0; i < sband->n_bitrates; i++)
		if (bitrates[i].flags & mandatory_flag)
			mandatory_rates |= BIT(i);
	return mandatory_rates;
}
EXPORT_SYMBOL(ieee80211_mandatory_rates);

68
int ieee80211_channel_to_frequency(int chan, enum ieee80211_band band)
69
{
70 71
	/* see 802.11 17.3.8.3.2 and Annex J
	 * there are overlapping channel numbers in 5GHz and 2GHz bands */
72 73 74 75
	if (chan <= 0)
		return 0; /* not supported */
	switch (band) {
	case IEEE80211_BAND_2GHZ:
76 77 78 79
		if (chan == 14)
			return 2484;
		else if (chan < 14)
			return 2407 + chan * 5;
80 81 82 83
		break;
	case IEEE80211_BAND_5GHZ:
		if (chan >= 182 && chan <= 196)
			return 4000 + chan * 5;
84
		else
85 86 87 88 89 90 91 92
			return 5000 + chan * 5;
		break;
	case IEEE80211_BAND_60GHZ:
		if (chan < 5)
			return 56160 + chan * 2160;
		break;
	default:
		;
93
	}
94
	return 0; /* not supported */
95 96 97 98 99
}
EXPORT_SYMBOL(ieee80211_channel_to_frequency);

int ieee80211_frequency_to_channel(int freq)
{
100
	/* see 802.11 17.3.8.3.2 and Annex J */
101 102
	if (freq == 2484)
		return 14;
103
	else if (freq < 2484)
104
		return (freq - 2407) / 5;
105 106
	else if (freq >= 4910 && freq <= 4980)
		return (freq - 4000) / 5;
107
	else if (freq <= 45000) /* DMG band lower limit */
108
		return (freq - 5000) / 5;
109 110 111 112
	else if (freq >= 58320 && freq <= 64800)
		return (freq - 56160) / 2160;
	else
		return 0;
113 114 115
}
EXPORT_SYMBOL(ieee80211_frequency_to_channel);

116 117
struct ieee80211_channel *__ieee80211_get_channel(struct wiphy *wiphy,
						  int freq)
118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136
{
	enum ieee80211_band band;
	struct ieee80211_supported_band *sband;
	int i;

	for (band = 0; band < IEEE80211_NUM_BANDS; band++) {
		sband = wiphy->bands[band];

		if (!sband)
			continue;

		for (i = 0; i < sband->n_channels; i++) {
			if (sband->channels[i].center_freq == freq)
				return &sband->channels[i];
		}
	}

	return NULL;
}
137
EXPORT_SYMBOL(__ieee80211_get_channel);
138

139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178
static void set_mandatory_flags_band(struct ieee80211_supported_band *sband,
				     enum ieee80211_band band)
{
	int i, want;

	switch (band) {
	case IEEE80211_BAND_5GHZ:
		want = 3;
		for (i = 0; i < sband->n_bitrates; i++) {
			if (sband->bitrates[i].bitrate == 60 ||
			    sband->bitrates[i].bitrate == 120 ||
			    sband->bitrates[i].bitrate == 240) {
				sband->bitrates[i].flags |=
					IEEE80211_RATE_MANDATORY_A;
				want--;
			}
		}
		WARN_ON(want);
		break;
	case IEEE80211_BAND_2GHZ:
		want = 7;
		for (i = 0; i < sband->n_bitrates; i++) {
			if (sband->bitrates[i].bitrate == 10) {
				sband->bitrates[i].flags |=
					IEEE80211_RATE_MANDATORY_B |
					IEEE80211_RATE_MANDATORY_G;
				want--;
			}

			if (sband->bitrates[i].bitrate == 20 ||
			    sband->bitrates[i].bitrate == 55 ||
			    sband->bitrates[i].bitrate == 110 ||
			    sband->bitrates[i].bitrate == 60 ||
			    sband->bitrates[i].bitrate == 120 ||
			    sband->bitrates[i].bitrate == 240) {
				sband->bitrates[i].flags |=
					IEEE80211_RATE_MANDATORY_G;
				want--;
			}

Johannes Berg's avatar
Johannes Berg committed
179 180 181 182
			if (sband->bitrates[i].bitrate != 10 &&
			    sband->bitrates[i].bitrate != 20 &&
			    sband->bitrates[i].bitrate != 55 &&
			    sband->bitrates[i].bitrate != 110)
183 184 185
				sband->bitrates[i].flags |=
					IEEE80211_RATE_ERP_G;
		}
186
		WARN_ON(want != 0 && want != 3 && want != 6);
187
		break;
188 189 190 191 192
	case IEEE80211_BAND_60GHZ:
		/* check for mandatory HT MCS 1..4 */
		WARN_ON(!sband->ht_cap.ht_supported);
		WARN_ON((sband->ht_cap.mcs.rx_mask[0] & 0x1e) != 0x1e);
		break;
193 194 195 196 197 198 199 200 201 202 203 204 205 206
	case IEEE80211_NUM_BANDS:
		WARN_ON(1);
		break;
	}
}

void ieee80211_set_bitrate_flags(struct wiphy *wiphy)
{
	enum ieee80211_band band;

	for (band = 0; band < IEEE80211_NUM_BANDS; band++)
		if (wiphy->bands[band])
			set_mandatory_flags_band(wiphy->bands[band], band);
}
207

208 209 210 211 212 213 214 215 216
bool cfg80211_supported_cipher_suite(struct wiphy *wiphy, u32 cipher)
{
	int i;
	for (i = 0; i < wiphy->n_cipher_suites; i++)
		if (cipher == wiphy->cipher_suites[i])
			return true;
	return false;
}

Johannes Berg's avatar
Johannes Berg committed
217 218
int cfg80211_validate_key_settings(struct cfg80211_registered_device *rdev,
				   struct key_params *params, int key_idx,
219
				   bool pairwise, const u8 *mac_addr)
220 221 222 223
{
	if (key_idx > 5)
		return -EINVAL;

224 225 226 227 228 229
	if (!pairwise && mac_addr && !(rdev->wiphy.flags & WIPHY_FLAG_IBSS_RSN))
		return -EINVAL;

	if (pairwise && !mac_addr)
		return -EINVAL;

230 231 232
	switch (params->cipher) {
	case WLAN_CIPHER_SUITE_TKIP:
	case WLAN_CIPHER_SUITE_CCMP:
233 234 235
	case WLAN_CIPHER_SUITE_CCMP_256:
	case WLAN_CIPHER_SUITE_GCMP:
	case WLAN_CIPHER_SUITE_GCMP_256:
236 237 238 239 240 241 242 243 244 245
		/* Disallow pairwise keys with non-zero index unless it's WEP
		 * or a vendor specific cipher (because current deployments use
		 * pairwise WEP keys with non-zero indices and for vendor
		 * specific ciphers this should be validated in the driver or
		 * hardware level - but 802.11i clearly specifies to use zero)
		 */
		if (pairwise && key_idx)
			return -EINVAL;
		break;
	case WLAN_CIPHER_SUITE_AES_CMAC:
246 247 248
	case WLAN_CIPHER_SUITE_BIP_CMAC_256:
	case WLAN_CIPHER_SUITE_BIP_GMAC_128:
	case WLAN_CIPHER_SUITE_BIP_GMAC_256:
249 250 251 252 253 254 255
		/* Disallow BIP (group-only) cipher as pairwise cipher */
		if (pairwise)
			return -EINVAL;
		break;
	default:
		break;
	}
256 257 258

	switch (params->cipher) {
	case WLAN_CIPHER_SUITE_WEP40:
259
		if (params->key_len != WLAN_KEY_LEN_WEP40)
260 261 262
			return -EINVAL;
		break;
	case WLAN_CIPHER_SUITE_TKIP:
263
		if (params->key_len != WLAN_KEY_LEN_TKIP)
264 265 266
			return -EINVAL;
		break;
	case WLAN_CIPHER_SUITE_CCMP:
267
		if (params->key_len != WLAN_KEY_LEN_CCMP)
268 269
			return -EINVAL;
		break;
270 271 272 273 274 275 276 277 278 279 280 281
	case WLAN_CIPHER_SUITE_CCMP_256:
		if (params->key_len != WLAN_KEY_LEN_CCMP_256)
			return -EINVAL;
		break;
	case WLAN_CIPHER_SUITE_GCMP:
		if (params->key_len != WLAN_KEY_LEN_GCMP)
			return -EINVAL;
		break;
	case WLAN_CIPHER_SUITE_GCMP_256:
		if (params->key_len != WLAN_KEY_LEN_GCMP_256)
			return -EINVAL;
		break;
282
	case WLAN_CIPHER_SUITE_WEP104:
283
		if (params->key_len != WLAN_KEY_LEN_WEP104)
284 285 286
			return -EINVAL;
		break;
	case WLAN_CIPHER_SUITE_AES_CMAC:
287
		if (params->key_len != WLAN_KEY_LEN_AES_CMAC)
288 289
			return -EINVAL;
		break;
290 291 292 293 294 295 296 297 298 299 300 301
	case WLAN_CIPHER_SUITE_BIP_CMAC_256:
		if (params->key_len != WLAN_KEY_LEN_BIP_CMAC_256)
			return -EINVAL;
		break;
	case WLAN_CIPHER_SUITE_BIP_GMAC_128:
		if (params->key_len != WLAN_KEY_LEN_BIP_GMAC_128)
			return -EINVAL;
		break;
	case WLAN_CIPHER_SUITE_BIP_GMAC_256:
		if (params->key_len != WLAN_KEY_LEN_BIP_GMAC_256)
			return -EINVAL;
		break;
302
	default:
303 304 305 306 307 308 309 310
		/*
		 * We don't know anything about this algorithm,
		 * allow using it -- but the driver must check
		 * all parameters! We still check below whether
		 * or not the driver supports this algorithm,
		 * of course.
		 */
		break;
311 312
	}

313 314 315 316 317 318 319 320
	if (params->seq) {
		switch (params->cipher) {
		case WLAN_CIPHER_SUITE_WEP40:
		case WLAN_CIPHER_SUITE_WEP104:
			/* These ciphers do not use key sequence */
			return -EINVAL;
		case WLAN_CIPHER_SUITE_TKIP:
		case WLAN_CIPHER_SUITE_CCMP:
321 322 323
		case WLAN_CIPHER_SUITE_CCMP_256:
		case WLAN_CIPHER_SUITE_GCMP:
		case WLAN_CIPHER_SUITE_GCMP_256:
324
		case WLAN_CIPHER_SUITE_AES_CMAC:
325 326 327
		case WLAN_CIPHER_SUITE_BIP_CMAC_256:
		case WLAN_CIPHER_SUITE_BIP_GMAC_128:
		case WLAN_CIPHER_SUITE_BIP_GMAC_256:
328 329 330 331 332 333
			if (params->seq_len != 6)
				return -EINVAL;
			break;
		}
	}

334
	if (!cfg80211_supported_cipher_suite(&rdev->wiphy, params->cipher))
Johannes Berg's avatar
Johannes Berg committed
335 336
		return -EINVAL;

337 338
	return 0;
}
339

340
unsigned int __attribute_const__ ieee80211_hdrlen(__le16 fc)
341 342 343 344 345 346
{
	unsigned int hdrlen = 24;

	if (ieee80211_is_data(fc)) {
		if (ieee80211_has_a4(fc))
			hdrlen = 30;
347
		if (ieee80211_is_data_qos(fc)) {
348
			hdrlen += IEEE80211_QOS_CTL_LEN;
349 350 351
			if (ieee80211_has_order(fc))
				hdrlen += IEEE80211_HT_CTL_LEN;
		}
352 353 354
		goto out;
	}

355 356 357 358 359 360
	if (ieee80211_is_mgmt(fc)) {
		if (ieee80211_has_order(fc))
			hdrlen += IEEE80211_HT_CTL_LEN;
		goto out;
	}

361 362 363 364 365 366 367 368 369 370 371 372 373 374 375 376 377 378 379 380 381 382 383 384 385 386 387 388 389 390 391 392 393 394 395
	if (ieee80211_is_ctl(fc)) {
		/*
		 * ACK and CTS are 10 bytes, all others 16. To see how
		 * to get this condition consider
		 *   subtype mask:   0b0000000011110000 (0x00F0)
		 *   ACK subtype:    0b0000000011010000 (0x00D0)
		 *   CTS subtype:    0b0000000011000000 (0x00C0)
		 *   bits that matter:         ^^^      (0x00E0)
		 *   value of those: 0b0000000011000000 (0x00C0)
		 */
		if ((fc & cpu_to_le16(0x00E0)) == cpu_to_le16(0x00C0))
			hdrlen = 10;
		else
			hdrlen = 16;
	}
out:
	return hdrlen;
}
EXPORT_SYMBOL(ieee80211_hdrlen);

unsigned int ieee80211_get_hdrlen_from_skb(const struct sk_buff *skb)
{
	const struct ieee80211_hdr *hdr =
			(const struct ieee80211_hdr *)skb->data;
	unsigned int hdrlen;

	if (unlikely(skb->len < 10))
		return 0;
	hdrlen = ieee80211_hdrlen(hdr->frame_control);
	if (unlikely(hdrlen > skb->len))
		return 0;
	return hdrlen;
}
EXPORT_SYMBOL(ieee80211_get_hdrlen_from_skb);

396
unsigned int ieee80211_get_mesh_hdrlen(struct ieee80211s_hdr *meshhdr)
397 398
{
	int ae = meshhdr->flags & MESH_FLAGS_AE;
399
	/* 802.11-2012, 8.2.4.7.3 */
400
	switch (ae) {
401
	default:
402 403
	case 0:
		return 6;
404
	case MESH_FLAGS_AE_A4:
405
		return 12;
406
	case MESH_FLAGS_AE_A5_A6:
407 408 409
		return 18;
	}
}
410
EXPORT_SYMBOL(ieee80211_get_mesh_hdrlen);
411

412
int ieee80211_data_to_8023(struct sk_buff *skb, const u8 *addr,
413 414 415 416 417 418 419 420 421 422 423 424 425 426 427 428 429 430 431 432 433 434 435 436 437 438 439 440 441
			   enum nl80211_iftype iftype)
{
	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
	u16 hdrlen, ethertype;
	u8 *payload;
	u8 dst[ETH_ALEN];
	u8 src[ETH_ALEN] __aligned(2);

	if (unlikely(!ieee80211_is_data_present(hdr->frame_control)))
		return -1;

	hdrlen = ieee80211_hdrlen(hdr->frame_control);

	/* convert IEEE 802.11 header + possible LLC headers into Ethernet
	 * header
	 * IEEE 802.11 address fields:
	 * ToDS FromDS Addr1 Addr2 Addr3 Addr4
	 *   0     0   DA    SA    BSSID n/a
	 *   0     1   DA    BSSID SA    n/a
	 *   1     0   BSSID SA    DA    n/a
	 *   1     1   RA    TA    DA    SA
	 */
	memcpy(dst, ieee80211_get_DA(hdr), ETH_ALEN);
	memcpy(src, ieee80211_get_SA(hdr), ETH_ALEN);

	switch (hdr->frame_control &
		cpu_to_le16(IEEE80211_FCTL_TODS | IEEE80211_FCTL_FROMDS)) {
	case cpu_to_le16(IEEE80211_FCTL_TODS):
		if (unlikely(iftype != NL80211_IFTYPE_AP &&
442 443
			     iftype != NL80211_IFTYPE_AP_VLAN &&
			     iftype != NL80211_IFTYPE_P2P_GO))
444 445 446 447
			return -1;
		break;
	case cpu_to_le16(IEEE80211_FCTL_TODS | IEEE80211_FCTL_FROMDS):
		if (unlikely(iftype != NL80211_IFTYPE_WDS &&
448 449 450
			     iftype != NL80211_IFTYPE_MESH_POINT &&
			     iftype != NL80211_IFTYPE_AP_VLAN &&
			     iftype != NL80211_IFTYPE_STATION))
451 452 453 454
			return -1;
		if (iftype == NL80211_IFTYPE_MESH_POINT) {
			struct ieee80211s_hdr *meshdr =
				(struct ieee80211s_hdr *) (skb->data + hdrlen);
Zhu Yi's avatar
Zhu Yi committed
455 456 457
			/* make sure meshdr->flags is on the linear part */
			if (!pskb_may_pull(skb, hdrlen + 1))
				return -1;
458 459
			if (meshdr->flags & MESH_FLAGS_AE_A4)
				return -1;
460
			if (meshdr->flags & MESH_FLAGS_AE_A5_A6) {
Zhu Yi's avatar
Zhu Yi committed
461 462 463 464 465 466
				skb_copy_bits(skb, hdrlen +
					offsetof(struct ieee80211s_hdr, eaddr1),
				       	dst, ETH_ALEN);
				skb_copy_bits(skb, hdrlen +
					offsetof(struct ieee80211s_hdr, eaddr2),
				        src, ETH_ALEN);
467
			}
Zhu Yi's avatar
Zhu Yi committed
468
			hdrlen += ieee80211_get_mesh_hdrlen(meshdr);
469 470 471
		}
		break;
	case cpu_to_le16(IEEE80211_FCTL_FROMDS):
472
		if ((iftype != NL80211_IFTYPE_STATION &&
473 474
		     iftype != NL80211_IFTYPE_P2P_CLIENT &&
		     iftype != NL80211_IFTYPE_MESH_POINT) ||
475
		    (is_multicast_ether_addr(dst) &&
476
		     ether_addr_equal(src, addr)))
477
			return -1;
478 479 480
		if (iftype == NL80211_IFTYPE_MESH_POINT) {
			struct ieee80211s_hdr *meshdr =
				(struct ieee80211s_hdr *) (skb->data + hdrlen);
Zhu Yi's avatar
Zhu Yi committed
481 482 483
			/* make sure meshdr->flags is on the linear part */
			if (!pskb_may_pull(skb, hdrlen + 1))
				return -1;
484 485
			if (meshdr->flags & MESH_FLAGS_AE_A5_A6)
				return -1;
486
			if (meshdr->flags & MESH_FLAGS_AE_A4)
Zhu Yi's avatar
Zhu Yi committed
487 488 489 490
				skb_copy_bits(skb, hdrlen +
					offsetof(struct ieee80211s_hdr, eaddr1),
					src, ETH_ALEN);
			hdrlen += ieee80211_get_mesh_hdrlen(meshdr);
491
		}
492 493
		break;
	case cpu_to_le16(0):
494
		if (iftype != NL80211_IFTYPE_ADHOC &&
495 496
		    iftype != NL80211_IFTYPE_STATION &&
		    iftype != NL80211_IFTYPE_OCB)
497
				return -1;
498 499 500
		break;
	}

Zhu Yi's avatar
Zhu Yi committed
501
	if (!pskb_may_pull(skb, hdrlen + 8))
502 503 504 505 506
		return -1;

	payload = skb->data + hdrlen;
	ethertype = (payload[6] << 8) | payload[7];

507
	if (likely((ether_addr_equal(payload, rfc1042_header) &&
508
		    ethertype != ETH_P_AARP && ethertype != ETH_P_IPX) ||
509
		   ether_addr_equal(payload, bridge_tunnel_header))) {
510 511 512 513 514 515 516 517 518 519 520 521 522 523 524 525 526 527 528 529
		/* remove RFC1042 or Bridge-Tunnel encapsulation and
		 * replace EtherType */
		skb_pull(skb, hdrlen + 6);
		memcpy(skb_push(skb, ETH_ALEN), src, ETH_ALEN);
		memcpy(skb_push(skb, ETH_ALEN), dst, ETH_ALEN);
	} else {
		struct ethhdr *ehdr;
		__be16 len;

		skb_pull(skb, hdrlen);
		len = htons(skb->len);
		ehdr = (struct ethhdr *) skb_push(skb, sizeof(struct ethhdr));
		memcpy(ehdr->h_dest, dst, ETH_ALEN);
		memcpy(ehdr->h_source, src, ETH_ALEN);
		ehdr->h_proto = len;
	}
	return 0;
}
EXPORT_SYMBOL(ieee80211_data_to_8023);

530
int ieee80211_data_from_8023(struct sk_buff *skb, const u8 *addr,
531 532
			     enum nl80211_iftype iftype,
			     const u8 *bssid, bool qos)
533 534 535 536 537 538 539 540 541 542 543 544 545 546 547 548 549 550 551 552 553 554 555
{
	struct ieee80211_hdr hdr;
	u16 hdrlen, ethertype;
	__le16 fc;
	const u8 *encaps_data;
	int encaps_len, skip_header_bytes;
	int nh_pos, h_pos;
	int head_need;

	if (unlikely(skb->len < ETH_HLEN))
		return -EINVAL;

	nh_pos = skb_network_header(skb) - skb->data;
	h_pos = skb_transport_header(skb) - skb->data;

	/* convert Ethernet header to proper 802.11 header (based on
	 * operation mode) */
	ethertype = (skb->data[12] << 8) | skb->data[13];
	fc = cpu_to_le16(IEEE80211_FTYPE_DATA | IEEE80211_STYPE_DATA);

	switch (iftype) {
	case NL80211_IFTYPE_AP:
	case NL80211_IFTYPE_AP_VLAN:
556
	case NL80211_IFTYPE_P2P_GO:
557 558 559 560 561 562 563 564
		fc |= cpu_to_le16(IEEE80211_FCTL_FROMDS);
		/* DA BSSID SA */
		memcpy(hdr.addr1, skb->data, ETH_ALEN);
		memcpy(hdr.addr2, addr, ETH_ALEN);
		memcpy(hdr.addr3, skb->data + ETH_ALEN, ETH_ALEN);
		hdrlen = 24;
		break;
	case NL80211_IFTYPE_STATION:
565
	case NL80211_IFTYPE_P2P_CLIENT:
566 567 568 569 570 571 572
		fc |= cpu_to_le16(IEEE80211_FCTL_TODS);
		/* BSSID SA DA */
		memcpy(hdr.addr1, bssid, ETH_ALEN);
		memcpy(hdr.addr2, skb->data + ETH_ALEN, ETH_ALEN);
		memcpy(hdr.addr3, skb->data, ETH_ALEN);
		hdrlen = 24;
		break;
573
	case NL80211_IFTYPE_OCB:
574 575 576 577 578 579 580 581 582 583 584 585 586 587 588 589 590 591 592 593 594 595 596 597 598
	case NL80211_IFTYPE_ADHOC:
		/* DA SA BSSID */
		memcpy(hdr.addr1, skb->data, ETH_ALEN);
		memcpy(hdr.addr2, skb->data + ETH_ALEN, ETH_ALEN);
		memcpy(hdr.addr3, bssid, ETH_ALEN);
		hdrlen = 24;
		break;
	default:
		return -EOPNOTSUPP;
	}

	if (qos) {
		fc |= cpu_to_le16(IEEE80211_STYPE_QOS_DATA);
		hdrlen += 2;
	}

	hdr.frame_control = fc;
	hdr.duration_id = 0;
	hdr.seq_ctrl = 0;

	skip_header_bytes = ETH_HLEN;
	if (ethertype == ETH_P_AARP || ethertype == ETH_P_IPX) {
		encaps_data = bridge_tunnel_header;
		encaps_len = sizeof(bridge_tunnel_header);
		skip_header_bytes -= 2;
Simon Horman's avatar
Simon Horman committed
599
	} else if (ethertype >= ETH_P_802_3_MIN) {
600 601 602 603 604 605 606 607 608 609 610 611 612 613 614 615 616 617 618
		encaps_data = rfc1042_header;
		encaps_len = sizeof(rfc1042_header);
		skip_header_bytes -= 2;
	} else {
		encaps_data = NULL;
		encaps_len = 0;
	}

	skb_pull(skb, skip_header_bytes);
	nh_pos -= skip_header_bytes;
	h_pos -= skip_header_bytes;

	head_need = hdrlen + encaps_len - skb_headroom(skb);

	if (head_need > 0 || skb_cloned(skb)) {
		head_need = max(head_need, 0);
		if (head_need)
			skb_orphan(skb);

619
		if (pskb_expand_head(skb, head_need, 0, GFP_ATOMIC))
620
			return -ENOMEM;
621

622 623 624 625 626 627 628 629 630 631 632 633 634 635 636 637 638 639 640 641 642 643 644 645 646
		skb->truesize += head_need;
	}

	if (encaps_data) {
		memcpy(skb_push(skb, encaps_len), encaps_data, encaps_len);
		nh_pos += encaps_len;
		h_pos += encaps_len;
	}

	memcpy(skb_push(skb, hdrlen), &hdr, hdrlen);

	nh_pos += hdrlen;
	h_pos += hdrlen;

	/* Update skb pointers to various headers since this modified frame
	 * is going to go through Linux networking code that may potentially
	 * need things like pointer to IP header. */
	skb_set_mac_header(skb, 0);
	skb_set_network_header(skb, nh_pos);
	skb_set_transport_header(skb, h_pos);

	return 0;
}
EXPORT_SYMBOL(ieee80211_data_from_8023);

647 648 649

void ieee80211_amsdu_to_8023s(struct sk_buff *skb, struct sk_buff_head *list,
			      const u8 *addr, enum nl80211_iftype iftype,
650 651
			      const unsigned int extra_headroom,
			      bool has_80211_header)
652 653 654 655 656 657 658 659
{
	struct sk_buff *frame = NULL;
	u16 ethertype;
	u8 *payload;
	const struct ethhdr *eth;
	int remaining, err;
	u8 dst[ETH_ALEN], src[ETH_ALEN];

660 661 662 663
	if (has_80211_header) {
		err = ieee80211_data_to_8023(skb, addr, iftype);
		if (err)
			goto out;
664

665 666 667 668 669 670 671
		/* skip the wrapping header */
		eth = (struct ethhdr *) skb_pull(skb, sizeof(struct ethhdr));
		if (!eth)
			goto out;
	} else {
		eth = (struct ethhdr *) skb->data;
	}
672 673 674 675 676 677 678 679 680 681 682 683 684 685 686 687 688 689 690 691 692 693 694 695 696 697 698 699 700 701 702 703 704 705 706 707 708 709 710 711 712 713 714 715 716 717 718 719

	while (skb != frame) {
		u8 padding;
		__be16 len = eth->h_proto;
		unsigned int subframe_len = sizeof(struct ethhdr) + ntohs(len);

		remaining = skb->len;
		memcpy(dst, eth->h_dest, ETH_ALEN);
		memcpy(src, eth->h_source, ETH_ALEN);

		padding = (4 - subframe_len) & 0x3;
		/* the last MSDU has no padding */
		if (subframe_len > remaining)
			goto purge;

		skb_pull(skb, sizeof(struct ethhdr));
		/* reuse skb for the last subframe */
		if (remaining <= subframe_len + padding)
			frame = skb;
		else {
			unsigned int hlen = ALIGN(extra_headroom, 4);
			/*
			 * Allocate and reserve two bytes more for payload
			 * alignment since sizeof(struct ethhdr) is 14.
			 */
			frame = dev_alloc_skb(hlen + subframe_len + 2);
			if (!frame)
				goto purge;

			skb_reserve(frame, hlen + sizeof(struct ethhdr) + 2);
			memcpy(skb_put(frame, ntohs(len)), skb->data,
				ntohs(len));

			eth = (struct ethhdr *)skb_pull(skb, ntohs(len) +
							padding);
			if (!eth) {
				dev_kfree_skb(frame);
				goto purge;
			}
		}

		skb_reset_network_header(frame);
		frame->dev = skb->dev;
		frame->priority = skb->priority;

		payload = frame->data;
		ethertype = (payload[6] << 8) | payload[7];

720
		if (likely((ether_addr_equal(payload, rfc1042_header) &&
721
			    ethertype != ETH_P_AARP && ethertype != ETH_P_IPX) ||
722
			   ether_addr_equal(payload, bridge_tunnel_header))) {
723 724 725 726 727 728 729 730 731 732 733 734 735 736 737 738 739 740 741 742 743 744 745
			/* remove RFC1042 or Bridge-Tunnel
			 * encapsulation and replace EtherType */
			skb_pull(frame, 6);
			memcpy(skb_push(frame, ETH_ALEN), src, ETH_ALEN);
			memcpy(skb_push(frame, ETH_ALEN), dst, ETH_ALEN);
		} else {
			memcpy(skb_push(frame, sizeof(__be16)), &len,
				sizeof(__be16));
			memcpy(skb_push(frame, ETH_ALEN), src, ETH_ALEN);
			memcpy(skb_push(frame, ETH_ALEN), dst, ETH_ALEN);
		}
		__skb_queue_tail(list, frame);
	}

	return;

 purge:
	__skb_queue_purge(list);
 out:
	dev_kfree_skb(skb);
}
EXPORT_SYMBOL(ieee80211_amsdu_to_8023s);

746
/* Given a data frame determine the 802.1p/1d tag to use. */
747 748
unsigned int cfg80211_classify8021d(struct sk_buff *skb,
				    struct cfg80211_qos_map *qos_map)
749 750
{
	unsigned int dscp;
751
	unsigned char vlan_priority;
752 753 754 755 756 757 758 759 760

	/* skb->priority values from 256->263 are magic values to
	 * directly indicate a specific 802.1d priority.  This is used
	 * to allow 802.1d priority to be passed directly in from VLAN
	 * tags, etc.
	 */
	if (skb->priority >= 256 && skb->priority <= 263)
		return skb->priority - 256;

761 762
	if (skb_vlan_tag_present(skb)) {
		vlan_priority = (skb_vlan_tag_get(skb) & VLAN_PRIO_MASK)
763 764 765 766 767
			>> VLAN_PRIO_SHIFT;
		if (vlan_priority > 0)
			return vlan_priority;
	}

768 769
	switch (skb->protocol) {
	case htons(ETH_P_IP):
770 771 772 773
		dscp = ipv4_get_dsfield(ip_hdr(skb)) & 0xfc;
		break;
	case htons(ETH_P_IPV6):
		dscp = ipv6_get_dsfield(ipv6_hdr(skb)) & 0xfc;
774
		break;
775 776 777 778 779 780 781 782 783 784 785 786 787 788 789
	case htons(ETH_P_MPLS_UC):
	case htons(ETH_P_MPLS_MC): {
		struct mpls_label mpls_tmp, *mpls;

		mpls = skb_header_pointer(skb, sizeof(struct ethhdr),
					  sizeof(*mpls), &mpls_tmp);
		if (!mpls)
			return 0;

		return (ntohl(mpls->entry) & MPLS_LS_TC_MASK)
			>> MPLS_LS_TC_SHIFT;
	}
	case htons(ETH_P_80221):
		/* 802.21 is always network control traffic */
		return 7;
790 791 792 793
	default:
		return 0;
	}

794 795 796 797 798 799 800 801 802 803 804 805 806 807 808
	if (qos_map) {
		unsigned int i, tmp_dscp = dscp >> 2;

		for (i = 0; i < qos_map->num_des; i++) {
			if (tmp_dscp == qos_map->dscp_exception[i].dscp)
				return qos_map->dscp_exception[i].up;
		}

		for (i = 0; i < 8; i++) {
			if (tmp_dscp >= qos_map->up[i].low &&
			    tmp_dscp <= qos_map->up[i].high)
				return i;
		}
	}

809 810 811
	return dscp >> 5;
}
EXPORT_SYMBOL(cfg80211_classify8021d);
812 813 814

const u8 *ieee80211_bss_get_ie(struct cfg80211_bss *bss, u8 ie)
{
815 816 817 818
	const struct cfg80211_bss_ies *ies;

	ies = rcu_dereference(bss->ies);
	if (!ies)
819
		return NULL;
820 821

	return cfg80211_find_ie(ie, ies->data, ies->len);
822 823
}
EXPORT_SYMBOL(ieee80211_bss_get_ie);
Johannes Berg's avatar
Johannes Berg committed
824 825 826

void cfg80211_upload_connect_keys(struct wireless_dev *wdev)
{
827
	struct cfg80211_registered_device *rdev = wiphy_to_rdev(wdev->wiphy);
Johannes Berg's avatar
Johannes Berg committed
828 829 830 831 832 833 834 835 836
	struct net_device *dev = wdev->netdev;
	int i;

	if (!wdev->connect_keys)
		return;

	for (i = 0; i < 6; i++) {
		if (!wdev->connect_keys->params[i].cipher)
			continue;
837 838
		if (rdev_add_key(rdev, dev, i, false, NULL,
				 &wdev->connect_keys->params[i])) {
839
			netdev_err(dev, "failed to set key %d\n", i);
840 841
			continue;
		}
Johannes Berg's avatar
Johannes Berg committed
842
		if (wdev->connect_keys->def == i)
843
			if (rdev_set_default_key(rdev, dev, i, true, true)) {
844
				netdev_err(dev, "failed to set defkey %d\n", i);
845 846
				continue;
			}
Johannes Berg's avatar
Johannes Berg committed
847
		if (wdev->connect_keys->defmgmt == i)
848
			if (rdev_set_default_mgmt_key(rdev, dev, i))
849
				netdev_err(dev, "failed to set mgtdef %d\n", i);
Johannes Berg's avatar
Johannes Berg committed
850 851
	}

852
	kzfree(wdev->connect_keys);
Johannes Berg's avatar
Johannes Berg committed
853 854
	wdev->connect_keys = NULL;
}
855

856
void cfg80211_process_wdev_events(struct wireless_dev *wdev)
857 858 859 860 861 862 863 864 865 866 867 868 869 870 871 872 873 874 875 876 877 878 879 880 881 882
{
	struct cfg80211_event *ev;
	unsigned long flags;
	const u8 *bssid = NULL;

	spin_lock_irqsave(&wdev->event_lock, flags);
	while (!list_empty(&wdev->event_list)) {
		ev = list_first_entry(&wdev->event_list,
				      struct cfg80211_event, list);
		list_del(&ev->list);
		spin_unlock_irqrestore(&wdev->event_lock, flags);

		wdev_lock(wdev);
		switch (ev->type) {
		case EVENT_CONNECT_RESULT:
			if (!is_zero_ether_addr(ev->cr.bssid))
				bssid = ev->cr.bssid;
			__cfg80211_connect_result(
				wdev->netdev, bssid,
				ev->cr.req_ie, ev->cr.req_ie_len,
				ev->cr.resp_ie, ev->cr.resp_ie_len,
				ev->cr.status,
				ev->cr.status == WLAN_STATUS_SUCCESS,
				NULL);
			break;
		case EVENT_ROAMED:
883 884 885
			__cfg80211_roamed(wdev, ev->rm.bss, ev->rm.req_ie,
					  ev->rm.req_ie_len, ev->rm.resp_ie,
					  ev->rm.resp_ie_len);
886 887 888 889
			break;
		case EVENT_DISCONNECTED:
			__cfg80211_disconnected(wdev->netdev,
						ev->dc.ie, ev->dc.ie_len,
890 891
						ev->dc.reason,
						!ev->dc.locally_generated);
892 893
			break;
		case EVENT_IBSS_JOINED:
894 895
			__cfg80211_ibss_joined(wdev->netdev, ev->ij.bssid,
					       ev->ij.channel);
896
			break;
897 898 899
		case EVENT_STOPPED:
			__cfg80211_leave(wiphy_to_rdev(wdev->wiphy), wdev);
			break;
900 901 902 903 904 905 906 907 908 909 910 911 912 913 914 915
		}
		wdev_unlock(wdev);

		kfree(ev);

		spin_lock_irqsave(&wdev->event_lock, flags);
	}
	spin_unlock_irqrestore(&wdev->event_lock, flags);
}

void cfg80211_process_rdev_events(struct cfg80211_registered_device *rdev)
{
	struct wireless_dev *wdev;

	ASSERT_RTNL();

916
	list_for_each_entry(wdev, &rdev->wdev_list, list)
917 918 919 920 921 922 923 924 925 926
		cfg80211_process_wdev_events(wdev);
}

int cfg80211_change_iface(struct cfg80211_registered_device *rdev,
			  struct net_device *dev, enum nl80211_iftype ntype,
			  u32 *flags, struct vif_params *params)
{
	int err;
	enum nl80211_iftype otype = dev->ieee80211_ptr->iftype;

927
	ASSERT_RTNL();
928 929 930 931 932

	/* don't support changing VLANs, you just re-create them */
	if (otype == NL80211_IFTYPE_AP_VLAN)
		return -EOPNOTSUPP;

933 934 935 936
	/* cannot change into P2P device type */
	if (ntype == NL80211_IFTYPE_P2P_DEVICE)
		return -EOPNOTSUPP;

937 938 939 940
	if (!rdev->ops->change_virtual_intf ||
	    !(rdev->wiphy.interface_modes & (1 << ntype)))
		return -EOPNOTSUPP;

941
	/* if it's part of a bridge, reject changing type to station/ibss */
942
	if ((dev->priv_flags & IFF_BRIDGE_PORT) &&
943 944 945
	    (ntype == NL80211_IFTYPE_ADHOC ||
	     ntype == NL80211_IFTYPE_STATION ||
	     ntype == NL80211_IFTYPE_P2P_CLIENT))
946 947
		return -EBUSY;

948
	if (ntype != otype && netif_running(dev)) {
949
		dev->ieee80211_ptr->use_4addr = false;
950
		dev->ieee80211_ptr->mesh_id_up_len = 0;
951
		wdev_lock(dev->ieee80211_ptr);
952
		rdev_set_qos_map(rdev, dev, NULL);
953
		wdev_unlock(dev->ieee80211_ptr);
954

955
		switch (otype) {
956
		case NL80211_IFTYPE_AP:
957
			cfg80211_stop_ap(rdev, dev, true);
958
			break;
959 960 961 962
		case NL80211_IFTYPE_ADHOC:
			cfg80211_leave_ibss(rdev, dev, false);
			break;
		case NL80211_IFTYPE_STATION:
963
		case NL80211_IFTYPE_P2P_CLIENT:
964
			wdev_lock(dev->ieee80211_ptr);
965 966
			cfg80211_disconnect(rdev, dev,
					    WLAN_REASON_DEAUTH_LEAVING, true);
967
			wdev_unlock(dev->ieee80211_ptr);
968 969 970 971 972 973 974 975 976 977 978
			break;
		case NL80211_IFTYPE_MESH_POINT:
			/* mesh should be handled? */
			break;
		default:
			break;
		}

		cfg80211_process_rdev_events(rdev);
	}

979
	err = rdev_change_virtual_intf(rdev, dev, ntype, flags, params);
980 981 982

	WARN_ON(!err && dev->ieee80211_ptr->iftype != ntype);

983 984 985
	if (!err && params && params->use_4addr != -1)
		dev->ieee80211_ptr->use_4addr = params->use_4addr;

986 987 988 989 990 991 992
	if (!err) {
		dev->priv_flags &= ~IFF_DONT_BRIDGE;
		switch (ntype) {
		case NL80211_IFTYPE_STATION:
			if (dev->ieee80211_ptr->use_4addr)
				break;
			/* fall through */
993
		case NL80211_IFTYPE_OCB:
994
		case NL80211_IFTYPE_P2P_CLIENT:
995 996 997
		case NL80211_IFTYPE_ADHOC:
			dev->priv_flags |= IFF_DONT_BRIDGE;
			break;
998
		case NL80211_IFTYPE_P2P_GO:
999 1000 1001 1002 1003 1004 1005 1006 1007 1008
		case NL80211_IFTYPE_AP:
		case NL80211_IFTYPE_AP_VLAN:
		case NL80211_IFTYPE_WDS:
		case NL80211_IFTYPE_MESH_POINT:
			/* bridging OK */
			break;
		case NL80211_IFTYPE_MONITOR:
			/* monitor can't bridge anyway */
			break;
		case NL80211_IFTYPE_UNSPECIFIED:
1009
		case NUM_NL80211_IFTYPES:
1010 1011
			/* not happening */
			break;
1012 1013 1014
		case NL80211_IFTYPE_P2P_DEVICE:
			WARN_ON(1);
			break;
1015 1016 1017
		}
	}

1018 1019 1020 1021 1022
	if (!err && ntype != otype && netif_running(dev)) {
		cfg80211_update_iface_num(rdev, ntype, 1);
		cfg80211_update_iface_num(rdev, otype, -1);
	}

1023 1024
	return err;
}
1025

1026 1027 1028 1029 1030 1031 1032 1033 1034 1035 1036 1037 1038 1039 1040 1041 1042 1043 1044 1045 1046 1047 1048 1049 1050 1051 1052 1053 1054 1055 1056 1057 1058 1059 1060 1061 1062 1063 1064 1065 1066 1067 1068 1069 1070 1071 1072
static u32 cfg80211_calculate_bitrate_60g(struct rate_info *rate)
{
	static const u32 __mcs2bitrate[] = {
		/* control PHY */
		[0] =   275,
		/* SC PHY */
		[1] =  3850,
		[2] =  7700,
		[3] =  9625,
		[4] = 11550,
		[5] = 12512, /* 1251.25 mbps */
		[6] = 15400,
		[7] = 19250,
		[8] = 23100,
		[9] = 25025,
		[10] = 30800,
		[11] = 38500,
		[12] = 46200,
		/* OFDM PHY */
		[13] =  6930,
		[14] =  8662, /* 866.25 mbps */
		[15] = 13860,
		[16] = 17325,
		[17] = 20790,
		[18] = 27720,
		[19] = 34650,
		[20] = 41580,
		[21] = 45045,
		[22] = 51975,
		[23] = 62370,
		[24] = 67568, /* 6756.75 mbps */
		/* LP-SC PHY */
		[25] =  6260,
		[26] =  8340,
		[27] = 11120,
		[28] = 12510,
		[29] = 16680,
		[30] = 22240,
		[31] = 25030,
	};

	if (WARN_ON_ONCE(rate->mcs >= ARRAY_SIZE(__mcs2bitrate)))
		return 0;

	return __mcs2bitrate[rate->mcs];
}

1073 1074 1075 1076 1077 1078 1079 1080 1081 1082 1083 1084 1085 1086 1087 1088 1089 1090 1091 1092 1093 1094 1095 1096 1097 1098 1099 1100 1101 1102 1103 1104 1105 1106 1107 1108 1109 1110 1111 1112 1113 1114 1115 1116 1117 1118 1119 1120 1121 1122 1123 1124 1125 1126
static u32 cfg80211_calculate_bitrate_vht(struct rate_info *rate)
{
	static const u32 base[4][10] = {
		{   6500000,
		   13000000,
		   19500000,
		   26000000,
		   39000000,
		   52000000,
		   58500000,
		   65000000,
		   78000000,
		   0,
		},
		{  13500000,
		   27000000,
		   40500000,
		   54000000,
		   81000000,
		  108000000,
		  121500000,
		  135000000,
		  162000000,
		  180000000,
		},
		{  29300000,
		   58500000,
		   87800000,
		  117000000,
		  175500000,
		  234000000,
		  263300000,
		  292500000,
		  351000000,
		  390000000,
		},
		{  58500000,
		  117000000,
		  175500000,
		  234000000,
		  351000000,
		  468000000,
		  526500000,
		  585000000,
		  702000000,
		  780000000,
		},
	};
	u32 bitrate;
	int idx;

	if (WARN_ON_ONCE(rate->mcs > 9))
		return 0;

1127 1128 1129 1130 1131 1132 1133 1134 1135 1136 1137 1138 1139 1140 1141 1142 1143 1144
	switch (rate->bw) {
	case RATE_INFO_BW_160:
		idx = 3;
		break;
	case RATE_INFO_BW_80:
		idx = 2;
		break;
	case RATE_INFO_BW_40:
		idx = 1;
		break;
	case RATE_INFO_BW_5:
	case RATE_INFO_BW_10:
	default:
		WARN_ON(1);
		/* fall through */
	case RATE_INFO_BW_20:
		idx = 0;
	}
1145 1146 1147 1148 1149 1150 1151 1152 1153 1154 1155

	bitrate = base[idx][rate->mcs];
	bitrate *= rate->nss;

	if (rate->flags & RATE_INFO_FLAGS_SHORT_GI)
		bitrate = (bitrate / 9) * 10;

	/* do NOT round down here */
	return (bitrate + 50000) / 100000;
}

1156
u32 cfg80211_calculate_bitrate(struct rate_info *rate)
1157 1158 1159
{
	int modulation, streams, bitrate;

1160 1161
	if (!(rate->flags & RATE_INFO_FLAGS_MCS) &&
	    !(rate->flags & RATE_INFO_FLAGS_VHT_MCS))
1162
		return rate->legacy;
1163 1164
	if (rate->flags & RATE_INFO_FLAGS_60G)
		return cfg80211_calculate_bitrate_60g(rate);
1165 1166
	if (rate->flags & RATE_INFO_FLAGS_VHT_MCS)
		return cfg80211_calculate_bitrate_vht(rate);
1167 1168

	/* the formula below does only work for MCS values smaller than 32 */
1169
	if (WARN_ON_ONCE(rate->mcs >= 32))
1170 1171 1172 1173 1174
		return 0;

	modulation = rate->mcs & 7;
	streams = (rate->mcs >> 3) + 1;

1175
	bitrate = (rate->bw == RATE_INFO_BW_40) ? 13500000 : 6500000;
1176 1177 1178 1179 1180 1181 1182 1183 1184 1185 1186 1187 1188 1189 1190 1191

	if (modulation < 4)
		bitrate *= (modulation + 1);
	else if (modulation == 4)
		bitrate *= (modulation + 2);
	else
		bitrate *= (modulation + 3);

	bitrate *= streams;

	if (rate->flags & RATE_INFO_FLAGS_SHORT_GI)
		bitrate = (bitrate / 9) * 10;

	/* do NOT round down here */
	return (bitrate + 50000) / 100000;
}
1192
EXPORT_SYMBOL(cfg80211_calculate_bitrate);
1193

1194 1195 1196
int cfg80211_get_p2p_attr(const u8 *ies, unsigned int len,
			  enum ieee80211_p2p_attr_id attr,
			  u8 *buf, unsigned int bufsize)
1197 1198 1199 1200 1201 1202 1203 1204 1205 1206 1207 1208 1209 1210 1211 1212 1213 1214 1215 1216 1217 1218 1219 1220 1221 1222 1223 1224 1225 1226 1227 1228 1229 1230 1231 1232 1233 1234 1235 1236 1237 1238 1239 1240 1241 1242 1243 1244 1245 1246 1247 1248 1249 1250 1251 1252 1253 1254 1255 1256 1257 1258 1259 1260 1261 1262 1263 1264 1265 1266 1267 1268 1269 1270 1271 1272 1273 1274 1275 1276 1277 1278 1279 1280 1281 1282 1283 1284 1285 1286 1287 1288 1289 1290 1291 1292 1293
{
	u8 *out = buf;
	u16 attr_remaining = 0;
	bool desired_attr = false;
	u16 desired_len = 0;

	while (len > 0) {
		unsigned int iedatalen;
		unsigned int copy;
		const u8 *iedata;

		if (len < 2)
			return -EILSEQ;
		iedatalen = ies[1];
		if (iedatalen + 2 > len)
			return -EILSEQ;

		if (ies[0] != WLAN_EID_VENDOR_SPECIFIC)
			goto cont;

		if (iedatalen < 4)
			goto cont;

		iedata = ies + 2;

		/* check WFA OUI, P2P subtype */
		if (iedata[0] != 0x50 || iedata[1] != 0x6f ||
		    iedata[2] != 0x9a || iedata[3] != 0x09)
			goto cont;

		iedatalen -= 4;
		iedata += 4;

		/* check attribute continuation into this IE */
		copy = min_t(unsigned int, attr_remaining, iedatalen);
		if (copy && desired_attr) {
			desired_len += copy;
			if (out) {
				memcpy(out, iedata, min(bufsize, copy));
				out += min(bufsize, copy);
				bufsize -= min(bufsize, copy);
			}


			if (copy == attr_remaining)
				return desired_len;
		}

		attr_remaining -= copy;
		if (attr_remaining)
			goto cont;

		iedatalen -= copy;
		iedata += copy;

		while (iedatalen > 0) {
			u16 attr_len;

			/* P2P attribute ID & size must fit */
			if (iedatalen < 3)
				return -EILSEQ;
			desired_attr = iedata[0] == attr;
			attr_len = get_unaligned_le16(iedata + 1);
			iedatalen -= 3;
			iedata += 3;

			copy = min_t(unsigned int, attr_len, iedatalen);

			if (desired_attr) {
				desired_len += copy;
				if (out) {
					memcpy(out, iedata, min(bufsize, copy));
					out += min(bufsize, copy);
					bufsize -= min(bufsize, copy);
				}

				if (copy == attr_len)
					return desired_len;
			}

			iedata += copy;
			iedatalen -= copy;
			attr_remaining = attr_len - copy;
		}

 cont:
		len -= ies[1] + 2;
		ies += ies[1] + 2;
	}

	if (attr_remaining && desired_attr)
		return -EILSEQ;

	return -ENOENT;
}
EXPORT_SYMBOL(cfg80211_get_p2p_attr);

1294 1295 1296 1297 1298 1299 1300 1301 1302 1303 1304 1305 1306 1307 1308 1309 1310 1311 1312 1313 1314 1315 1316 1317 1318 1319 1320 1321 1322 1323 1324 1325 1326 1327 1328 1329 1330 1331 1332 1333 1334
static bool ieee80211_id_in_list(const u8 *ids, int n_ids, u8 id