util.c 26 KB

1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859606162636465666768697071727374757677787980818283848586878889909192939495969798991001011021031041051061071081091101111121131141151161171181191201211221231241251261271281291301311321331341351361371381391401411421431441451461471481491501511521531541551561571581591601611621631641651661671681691701711721731741751761771781791801811821831841851861871881891901911921931941951961971981992002012022032042052062072082092102112122132142152162172182192202212222232242252262272282292302312322332342352362372382392402412422432442452462472482492502512522532542552562572582592602612622632642652662672682692702712722732742752762772782792802812822832842852862872882892902912922932942952962972982993003013023033043053063073083093103113123133143153163173183193203213223233243253263273283293303313323333343353363373383393403413423433443453463473483493503513523533543553563573583593603613623633643653663673683693703713723733743753763773783793803813823833843853863873883893903913923933943953963973983994004014024034044054064074084094104114124134144154164174184194204214224234244254264274284294304314324334344354364374384394404414424434444454464474484494504514524534544554564574584594604614624634644654664674684694704714724734744754764774784794804814824834844854864874884894904914924934944954964974984995005015025035045055065075085095105115125135145155165175185195205215225235245255265275285295305315325335345355365375385395405415425435445455465475485495505515525535545555565575585595605615625635645655665675685695705715725735745755765775785795805815825835845855865875885895905915925935945955965975985996006016026036046056066076086096106116126136146156166176186196206216226236246256266276286296306316326336346356366376386396406416426436446456466476486496506516526536546556566576586596606616626636646656666676686696706716726736746756766776786796806816826836846856866876886896906916926936946956966976986997007017027037047057067077087097107117127137147157167177187197207217227237247257267277287297307317327337347357367377387397407417427437447457467477487497507517527537547557567577587597607617627637647657667677687697707717727737747757767777787797807817827837847857867877887897907917927937947957967977987998008018028038048058068078088098108118128138148158168178188198208218228238248258268278288298308318328338348358368378388398408418428438448458468478488498508518528538548558568578588598608618628638648658668678688698708718728738748758768778788798808818828838848858868878888898908918928938948958968978988999009019029039049059069079089099109119129139149159169179189199209219229239249259269279289299309319329339349359369379389399409419429439449459469479489499509519529539549559569579589599609619629639649659669679689699709719729739749759769779789799809819829839849859869879889899909919929939949959969979989991000100110021003100410051006100710081009101010111012101310141015101610171018101910201021102210231024102510261027102810291030103110321033103410351036103710381039104010411042104310441045104610471048104910501051
  1. /*
  2. * Wireless utility functions
  3. *
  4. * Copyright 2007-2009 Johannes Berg <johannes@sipsolutions.net>
  5. */
  6. #include <linux/export.h>
  7. #include <linux/bitops.h>
  8. #include <linux/etherdevice.h>
  9. #include <linux/slab.h>
  10. #include <net/cfg80211.h>
  11. #include <net/ip.h>
  12. #include "core.h"
  13. struct ieee80211_rate *
  14. ieee80211_get_response_rate(struct ieee80211_supported_band *sband,
  15. u32 basic_rates, int bitrate)
  16. {
  17. struct ieee80211_rate *result = &sband->bitrates[0];
  18. int i;
  19. for (i = 0; i < sband->n_bitrates; i++) {
  20. if (!(basic_rates & BIT(i)))
  21. continue;
  22. if (sband->bitrates[i].bitrate > bitrate)
  23. continue;
  24. result = &sband->bitrates[i];
  25. }
  26. return result;
  27. }
  28. EXPORT_SYMBOL(ieee80211_get_response_rate);
  29. int ieee80211_channel_to_frequency(int chan, enum ieee80211_band band)
  30. {
  31. /* see 802.11 17.3.8.3.2 and Annex J
  32. * there are overlapping channel numbers in 5GHz and 2GHz bands */
  33. if (band == IEEE80211_BAND_5GHZ) {
  34. if (chan >= 182 && chan <= 196)
  35. return 4000 + chan * 5;
  36. else
  37. return 5000 + chan * 5;
  38. } else { /* IEEE80211_BAND_2GHZ */
  39. if (chan == 14)
  40. return 2484;
  41. else if (chan < 14)
  42. return 2407 + chan * 5;
  43. else
  44. return 0; /* not supported */
  45. }
  46. }
  47. EXPORT_SYMBOL(ieee80211_channel_to_frequency);
  48. int ieee80211_frequency_to_channel(int freq)
  49. {
  50. /* see 802.11 17.3.8.3.2 and Annex J */
  51. if (freq == 2484)
  52. return 14;
  53. else if (freq < 2484)
  54. return (freq - 2407) / 5;
  55. else if (freq >= 4910 && freq <= 4980)
  56. return (freq - 4000) / 5;
  57. else
  58. return (freq - 5000) / 5;
  59. }
  60. EXPORT_SYMBOL(ieee80211_frequency_to_channel);
  61. struct ieee80211_channel *__ieee80211_get_channel(struct wiphy *wiphy,
  62. int freq)
  63. {
  64. enum ieee80211_band band;
  65. struct ieee80211_supported_band *sband;
  66. int i;
  67. for (band = 0; band < IEEE80211_NUM_BANDS; band++) {
  68. sband = wiphy->bands[band];
  69. if (!sband)
  70. continue;
  71. for (i = 0; i < sband->n_channels; i++) {
  72. if (sband->channels[i].center_freq == freq)
  73. return &sband->channels[i];
  74. }
  75. }
  76. return NULL;
  77. }
  78. EXPORT_SYMBOL(__ieee80211_get_channel);
  79. static void set_mandatory_flags_band(struct ieee80211_supported_band *sband,
  80. enum ieee80211_band band)
  81. {
  82. int i, want;
  83. switch (band) {
  84. case IEEE80211_BAND_5GHZ:
  85. want = 3;
  86. for (i = 0; i < sband->n_bitrates; i++) {
  87. if (sband->bitrates[i].bitrate == 60 ||
  88. sband->bitrates[i].bitrate == 120 ||
  89. sband->bitrates[i].bitrate == 240) {
  90. sband->bitrates[i].flags |=
  91. IEEE80211_RATE_MANDATORY_A;
  92. want--;
  93. }
  94. }
  95. WARN_ON(want);
  96. break;
  97. case IEEE80211_BAND_2GHZ:
  98. want = 7;
  99. for (i = 0; i < sband->n_bitrates; i++) {
  100. if (sband->bitrates[i].bitrate == 10) {
  101. sband->bitrates[i].flags |=
  102. IEEE80211_RATE_MANDATORY_B |
  103. IEEE80211_RATE_MANDATORY_G;
  104. want--;
  105. }
  106. if (sband->bitrates[i].bitrate == 20 ||
  107. sband->bitrates[i].bitrate == 55 ||
  108. sband->bitrates[i].bitrate == 110 ||
  109. sband->bitrates[i].bitrate == 60 ||
  110. sband->bitrates[i].bitrate == 120 ||
  111. sband->bitrates[i].bitrate == 240) {
  112. sband->bitrates[i].flags |=
  113. IEEE80211_RATE_MANDATORY_G;
  114. want--;
  115. }
  116. if (sband->bitrates[i].bitrate != 10 &&
  117. sband->bitrates[i].bitrate != 20 &&
  118. sband->bitrates[i].bitrate != 55 &&
  119. sband->bitrates[i].bitrate != 110)
  120. sband->bitrates[i].flags |=
  121. IEEE80211_RATE_ERP_G;
  122. }
  123. WARN_ON(want != 0 && want != 3 && want != 6);
  124. break;
  125. case IEEE80211_NUM_BANDS:
  126. WARN_ON(1);
  127. break;
  128. }
  129. }
  130. void ieee80211_set_bitrate_flags(struct wiphy *wiphy)
  131. {
  132. enum ieee80211_band band;
  133. for (band = 0; band < IEEE80211_NUM_BANDS; band++)
  134. if (wiphy->bands[band])
  135. set_mandatory_flags_band(wiphy->bands[band], band);
  136. }
  137. bool cfg80211_supported_cipher_suite(struct wiphy *wiphy, u32 cipher)
  138. {
  139. int i;
  140. for (i = 0; i < wiphy->n_cipher_suites; i++)
  141. if (cipher == wiphy->cipher_suites[i])
  142. return true;
  143. return false;
  144. }
  145. int cfg80211_validate_key_settings(struct cfg80211_registered_device *rdev,
  146. struct key_params *params, int key_idx,
  147. bool pairwise, const u8 *mac_addr)
  148. {
  149. if (key_idx > 5)
  150. return -EINVAL;
  151. if (!pairwise && mac_addr && !(rdev->wiphy.flags & WIPHY_FLAG_IBSS_RSN))
  152. return -EINVAL;
  153. if (pairwise && !mac_addr)
  154. return -EINVAL;
  155. /*
  156. * Disallow pairwise keys with non-zero index unless it's WEP
  157. * or a vendor specific cipher (because current deployments use
  158. * pairwise WEP keys with non-zero indices and for vendor specific
  159. * ciphers this should be validated in the driver or hardware level
  160. * - but 802.11i clearly specifies to use zero)
  161. */
  162. if (pairwise && key_idx &&
  163. ((params->cipher == WLAN_CIPHER_SUITE_TKIP) ||
  164. (params->cipher == WLAN_CIPHER_SUITE_CCMP) ||
  165. (params->cipher == WLAN_CIPHER_SUITE_AES_CMAC)))
  166. return -EINVAL;
  167. switch (params->cipher) {
  168. case WLAN_CIPHER_SUITE_WEP40:
  169. if (params->key_len != WLAN_KEY_LEN_WEP40)
  170. return -EINVAL;
  171. break;
  172. case WLAN_CIPHER_SUITE_TKIP:
  173. if (params->key_len != WLAN_KEY_LEN_TKIP)
  174. return -EINVAL;
  175. break;
  176. case WLAN_CIPHER_SUITE_CCMP:
  177. if (params->key_len != WLAN_KEY_LEN_CCMP)
  178. return -EINVAL;
  179. break;
  180. case WLAN_CIPHER_SUITE_WEP104:
  181. if (params->key_len != WLAN_KEY_LEN_WEP104)
  182. return -EINVAL;
  183. break;
  184. case WLAN_CIPHER_SUITE_AES_CMAC:
  185. if (params->key_len != WLAN_KEY_LEN_AES_CMAC)
  186. return -EINVAL;
  187. break;
  188. default:
  189. /*
  190. * We don't know anything about this algorithm,
  191. * allow using it -- but the driver must check
  192. * all parameters! We still check below whether
  193. * or not the driver supports this algorithm,
  194. * of course.
  195. */
  196. break;
  197. }
  198. if (params->seq) {
  199. switch (params->cipher) {
  200. case WLAN_CIPHER_SUITE_WEP40:
  201. case WLAN_CIPHER_SUITE_WEP104:
  202. /* These ciphers do not use key sequence */
  203. return -EINVAL;
  204. case WLAN_CIPHER_SUITE_TKIP:
  205. case WLAN_CIPHER_SUITE_CCMP:
  206. case WLAN_CIPHER_SUITE_AES_CMAC:
  207. if (params->seq_len != 6)
  208. return -EINVAL;
  209. break;
  210. }
  211. }
  212. if (!cfg80211_supported_cipher_suite(&rdev->wiphy, params->cipher))
  213. return -EINVAL;
  214. return 0;
  215. }
  216. unsigned int __attribute_const__ ieee80211_hdrlen(__le16 fc)
  217. {
  218. unsigned int hdrlen = 24;
  219. if (ieee80211_is_data(fc)) {
  220. if (ieee80211_has_a4(fc))
  221. hdrlen = 30;
  222. if (ieee80211_is_data_qos(fc)) {
  223. hdrlen += IEEE80211_QOS_CTL_LEN;
  224. if (ieee80211_has_order(fc))
  225. hdrlen += IEEE80211_HT_CTL_LEN;
  226. }
  227. goto out;
  228. }
  229. if (ieee80211_is_ctl(fc)) {
  230. /*
  231. * ACK and CTS are 10 bytes, all others 16. To see how
  232. * to get this condition consider
  233. * subtype mask: 0b0000000011110000 (0x00F0)
  234. * ACK subtype: 0b0000000011010000 (0x00D0)
  235. * CTS subtype: 0b0000000011000000 (0x00C0)
  236. * bits that matter: ^^^ (0x00E0)
  237. * value of those: 0b0000000011000000 (0x00C0)
  238. */
  239. if ((fc & cpu_to_le16(0x00E0)) == cpu_to_le16(0x00C0))
  240. hdrlen = 10;
  241. else
  242. hdrlen = 16;
  243. }
  244. out:
  245. return hdrlen;
  246. }
  247. EXPORT_SYMBOL(ieee80211_hdrlen);
  248. unsigned int ieee80211_get_hdrlen_from_skb(const struct sk_buff *skb)
  249. {
  250. const struct ieee80211_hdr *hdr =
  251. (const struct ieee80211_hdr *)skb->data;
  252. unsigned int hdrlen;
  253. if (unlikely(skb->len < 10))
  254. return 0;
  255. hdrlen = ieee80211_hdrlen(hdr->frame_control);
  256. if (unlikely(hdrlen > skb->len))
  257. return 0;
  258. return hdrlen;
  259. }
  260. EXPORT_SYMBOL(ieee80211_get_hdrlen_from_skb);
  261. static int ieee80211_get_mesh_hdrlen(struct ieee80211s_hdr *meshhdr)
  262. {
  263. int ae = meshhdr->flags & MESH_FLAGS_AE;
  264. /* 7.1.3.5a.2 */
  265. switch (ae) {
  266. case 0:
  267. return 6;
  268. case MESH_FLAGS_AE_A4:
  269. return 12;
  270. case MESH_FLAGS_AE_A5_A6:
  271. return 18;
  272. case (MESH_FLAGS_AE_A4 | MESH_FLAGS_AE_A5_A6):
  273. return 24;
  274. default:
  275. return 6;
  276. }
  277. }
  278. int ieee80211_data_to_8023(struct sk_buff *skb, const u8 *addr,
  279. enum nl80211_iftype iftype)
  280. {
  281. struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
  282. u16 hdrlen, ethertype;
  283. u8 *payload;
  284. u8 dst[ETH_ALEN];
  285. u8 src[ETH_ALEN] __aligned(2);
  286. if (unlikely(!ieee80211_is_data_present(hdr->frame_control)))
  287. return -1;
  288. hdrlen = ieee80211_hdrlen(hdr->frame_control);
  289. /* convert IEEE 802.11 header + possible LLC headers into Ethernet
  290. * header
  291. * IEEE 802.11 address fields:
  292. * ToDS FromDS Addr1 Addr2 Addr3 Addr4
  293. * 0 0 DA SA BSSID n/a
  294. * 0 1 DA BSSID SA n/a
  295. * 1 0 BSSID SA DA n/a
  296. * 1 1 RA TA DA SA
  297. */
  298. memcpy(dst, ieee80211_get_DA(hdr), ETH_ALEN);
  299. memcpy(src, ieee80211_get_SA(hdr), ETH_ALEN);
  300. switch (hdr->frame_control &
  301. cpu_to_le16(IEEE80211_FCTL_TODS | IEEE80211_FCTL_FROMDS)) {
  302. case cpu_to_le16(IEEE80211_FCTL_TODS):
  303. if (unlikely(iftype != NL80211_IFTYPE_AP &&
  304. iftype != NL80211_IFTYPE_AP_VLAN &&
  305. iftype != NL80211_IFTYPE_P2P_GO))
  306. return -1;
  307. break;
  308. case cpu_to_le16(IEEE80211_FCTL_TODS | IEEE80211_FCTL_FROMDS):
  309. if (unlikely(iftype != NL80211_IFTYPE_WDS &&
  310. iftype != NL80211_IFTYPE_MESH_POINT &&
  311. iftype != NL80211_IFTYPE_AP_VLAN &&
  312. iftype != NL80211_IFTYPE_STATION))
  313. return -1;
  314. if (iftype == NL80211_IFTYPE_MESH_POINT) {
  315. struct ieee80211s_hdr *meshdr =
  316. (struct ieee80211s_hdr *) (skb->data + hdrlen);
  317. /* make sure meshdr->flags is on the linear part */
  318. if (!pskb_may_pull(skb, hdrlen + 1))
  319. return -1;
  320. if (meshdr->flags & MESH_FLAGS_AE_A5_A6) {
  321. skb_copy_bits(skb, hdrlen +
  322. offsetof(struct ieee80211s_hdr, eaddr1),
  323. dst, ETH_ALEN);
  324. skb_copy_bits(skb, hdrlen +
  325. offsetof(struct ieee80211s_hdr, eaddr2),
  326. src, ETH_ALEN);
  327. }
  328. hdrlen += ieee80211_get_mesh_hdrlen(meshdr);
  329. }
  330. break;
  331. case cpu_to_le16(IEEE80211_FCTL_FROMDS):
  332. if ((iftype != NL80211_IFTYPE_STATION &&
  333. iftype != NL80211_IFTYPE_P2P_CLIENT &&
  334. iftype != NL80211_IFTYPE_MESH_POINT) ||
  335. (is_multicast_ether_addr(dst) &&
  336. !compare_ether_addr(src, addr)))
  337. return -1;
  338. if (iftype == NL80211_IFTYPE_MESH_POINT) {
  339. struct ieee80211s_hdr *meshdr =
  340. (struct ieee80211s_hdr *) (skb->data + hdrlen);
  341. /* make sure meshdr->flags is on the linear part */
  342. if (!pskb_may_pull(skb, hdrlen + 1))
  343. return -1;
  344. if (meshdr->flags & MESH_FLAGS_AE_A4)
  345. skb_copy_bits(skb, hdrlen +
  346. offsetof(struct ieee80211s_hdr, eaddr1),
  347. src, ETH_ALEN);
  348. hdrlen += ieee80211_get_mesh_hdrlen(meshdr);
  349. }
  350. break;
  351. case cpu_to_le16(0):
  352. if (iftype != NL80211_IFTYPE_ADHOC &&
  353. iftype != NL80211_IFTYPE_STATION)
  354. return -1;
  355. break;
  356. }
  357. if (!pskb_may_pull(skb, hdrlen + 8))
  358. return -1;
  359. payload = skb->data + hdrlen;
  360. ethertype = (payload[6] << 8) | payload[7];
  361. if (likely((compare_ether_addr(payload, rfc1042_header) == 0 &&
  362. ethertype != ETH_P_AARP && ethertype != ETH_P_IPX) ||
  363. compare_ether_addr(payload, bridge_tunnel_header) == 0)) {
  364. /* remove RFC1042 or Bridge-Tunnel encapsulation and
  365. * replace EtherType */
  366. skb_pull(skb, hdrlen + 6);
  367. memcpy(skb_push(skb, ETH_ALEN), src, ETH_ALEN);
  368. memcpy(skb_push(skb, ETH_ALEN), dst, ETH_ALEN);
  369. } else {
  370. struct ethhdr *ehdr;
  371. __be16 len;
  372. skb_pull(skb, hdrlen);
  373. len = htons(skb->len);
  374. ehdr = (struct ethhdr *) skb_push(skb, sizeof(struct ethhdr));
  375. memcpy(ehdr->h_dest, dst, ETH_ALEN);
  376. memcpy(ehdr->h_source, src, ETH_ALEN);
  377. ehdr->h_proto = len;
  378. }
  379. return 0;
  380. }
  381. EXPORT_SYMBOL(ieee80211_data_to_8023);
  382. int ieee80211_data_from_8023(struct sk_buff *skb, const u8 *addr,
  383. enum nl80211_iftype iftype, u8 *bssid, bool qos)
  384. {
  385. struct ieee80211_hdr hdr;
  386. u16 hdrlen, ethertype;
  387. __le16 fc;
  388. const u8 *encaps_data;
  389. int encaps_len, skip_header_bytes;
  390. int nh_pos, h_pos;
  391. int head_need;
  392. if (unlikely(skb->len < ETH_HLEN))
  393. return -EINVAL;
  394. nh_pos = skb_network_header(skb) - skb->data;
  395. h_pos = skb_transport_header(skb) - skb->data;
  396. /* convert Ethernet header to proper 802.11 header (based on
  397. * operation mode) */
  398. ethertype = (skb->data[12] << 8) | skb->data[13];
  399. fc = cpu_to_le16(IEEE80211_FTYPE_DATA | IEEE80211_STYPE_DATA);
  400. switch (iftype) {
  401. case NL80211_IFTYPE_AP:
  402. case NL80211_IFTYPE_AP_VLAN:
  403. case NL80211_IFTYPE_P2P_GO:
  404. fc |= cpu_to_le16(IEEE80211_FCTL_FROMDS);
  405. /* DA BSSID SA */
  406. memcpy(hdr.addr1, skb->data, ETH_ALEN);
  407. memcpy(hdr.addr2, addr, ETH_ALEN);
  408. memcpy(hdr.addr3, skb->data + ETH_ALEN, ETH_ALEN);
  409. hdrlen = 24;
  410. break;
  411. case NL80211_IFTYPE_STATION:
  412. case NL80211_IFTYPE_P2P_CLIENT:
  413. fc |= cpu_to_le16(IEEE80211_FCTL_TODS);
  414. /* BSSID SA DA */
  415. memcpy(hdr.addr1, bssid, ETH_ALEN);
  416. memcpy(hdr.addr2, skb->data + ETH_ALEN, ETH_ALEN);
  417. memcpy(hdr.addr3, skb->data, ETH_ALEN);
  418. hdrlen = 24;
  419. break;
  420. case NL80211_IFTYPE_ADHOC:
  421. /* DA SA BSSID */
  422. memcpy(hdr.addr1, skb->data, ETH_ALEN);
  423. memcpy(hdr.addr2, skb->data + ETH_ALEN, ETH_ALEN);
  424. memcpy(hdr.addr3, bssid, ETH_ALEN);
  425. hdrlen = 24;
  426. break;
  427. default:
  428. return -EOPNOTSUPP;
  429. }
  430. if (qos) {
  431. fc |= cpu_to_le16(IEEE80211_STYPE_QOS_DATA);
  432. hdrlen += 2;
  433. }
  434. hdr.frame_control = fc;
  435. hdr.duration_id = 0;
  436. hdr.seq_ctrl = 0;
  437. skip_header_bytes = ETH_HLEN;
  438. if (ethertype == ETH_P_AARP || ethertype == ETH_P_IPX) {
  439. encaps_data = bridge_tunnel_header;
  440. encaps_len = sizeof(bridge_tunnel_header);
  441. skip_header_bytes -= 2;
  442. } else if (ethertype > 0x600) {
  443. encaps_data = rfc1042_header;
  444. encaps_len = sizeof(rfc1042_header);
  445. skip_header_bytes -= 2;
  446. } else {
  447. encaps_data = NULL;
  448. encaps_len = 0;
  449. }
  450. skb_pull(skb, skip_header_bytes);
  451. nh_pos -= skip_header_bytes;
  452. h_pos -= skip_header_bytes;
  453. head_need = hdrlen + encaps_len - skb_headroom(skb);
  454. if (head_need > 0 || skb_cloned(skb)) {
  455. head_need = max(head_need, 0);
  456. if (head_need)
  457. skb_orphan(skb);
  458. if (pskb_expand_head(skb, head_need, 0, GFP_ATOMIC))
  459. return -ENOMEM;
  460. skb->truesize += head_need;
  461. }
  462. if (encaps_data) {
  463. memcpy(skb_push(skb, encaps_len), encaps_data, encaps_len);
  464. nh_pos += encaps_len;
  465. h_pos += encaps_len;
  466. }
  467. memcpy(skb_push(skb, hdrlen), &hdr, hdrlen);
  468. nh_pos += hdrlen;
  469. h_pos += hdrlen;
  470. /* Update skb pointers to various headers since this modified frame
  471. * is going to go through Linux networking code that may potentially
  472. * need things like pointer to IP header. */
  473. skb_set_mac_header(skb, 0);
  474. skb_set_network_header(skb, nh_pos);
  475. skb_set_transport_header(skb, h_pos);
  476. return 0;
  477. }
  478. EXPORT_SYMBOL(ieee80211_data_from_8023);
  479. void ieee80211_amsdu_to_8023s(struct sk_buff *skb, struct sk_buff_head *list,
  480. const u8 *addr, enum nl80211_iftype iftype,
  481. const unsigned int extra_headroom,
  482. bool has_80211_header)
  483. {
  484. struct sk_buff *frame = NULL;
  485. u16 ethertype;
  486. u8 *payload;
  487. const struct ethhdr *eth;
  488. int remaining, err;
  489. u8 dst[ETH_ALEN], src[ETH_ALEN];
  490. if (has_80211_header) {
  491. err = ieee80211_data_to_8023(skb, addr, iftype);
  492. if (err)
  493. goto out;
  494. /* skip the wrapping header */
  495. eth = (struct ethhdr *) skb_pull(skb, sizeof(struct ethhdr));
  496. if (!eth)
  497. goto out;
  498. } else {
  499. eth = (struct ethhdr *) skb->data;
  500. }
  501. while (skb != frame) {
  502. u8 padding;
  503. __be16 len = eth->h_proto;
  504. unsigned int subframe_len = sizeof(struct ethhdr) + ntohs(len);
  505. remaining = skb->len;
  506. memcpy(dst, eth->h_dest, ETH_ALEN);
  507. memcpy(src, eth->h_source, ETH_ALEN);
  508. padding = (4 - subframe_len) & 0x3;
  509. /* the last MSDU has no padding */
  510. if (subframe_len > remaining)
  511. goto purge;
  512. skb_pull(skb, sizeof(struct ethhdr));
  513. /* reuse skb for the last subframe */
  514. if (remaining <= subframe_len + padding)
  515. frame = skb;
  516. else {
  517. unsigned int hlen = ALIGN(extra_headroom, 4);
  518. /*
  519. * Allocate and reserve two bytes more for payload
  520. * alignment since sizeof(struct ethhdr) is 14.
  521. */
  522. frame = dev_alloc_skb(hlen + subframe_len + 2);
  523. if (!frame)
  524. goto purge;
  525. skb_reserve(frame, hlen + sizeof(struct ethhdr) + 2);
  526. memcpy(skb_put(frame, ntohs(len)), skb->data,
  527. ntohs(len));
  528. eth = (struct ethhdr *)skb_pull(skb, ntohs(len) +
  529. padding);
  530. if (!eth) {
  531. dev_kfree_skb(frame);
  532. goto purge;
  533. }
  534. }
  535. skb_reset_network_header(frame);
  536. frame->dev = skb->dev;
  537. frame->priority = skb->priority;
  538. payload = frame->data;
  539. ethertype = (payload[6] << 8) | payload[7];
  540. if (likely((compare_ether_addr(payload, rfc1042_header) == 0 &&
  541. ethertype != ETH_P_AARP && ethertype != ETH_P_IPX) ||
  542. compare_ether_addr(payload,
  543. bridge_tunnel_header) == 0)) {
  544. /* remove RFC1042 or Bridge-Tunnel
  545. * encapsulation and replace EtherType */
  546. skb_pull(frame, 6);
  547. memcpy(skb_push(frame, ETH_ALEN), src, ETH_ALEN);
  548. memcpy(skb_push(frame, ETH_ALEN), dst, ETH_ALEN);
  549. } else {
  550. memcpy(skb_push(frame, sizeof(__be16)), &len,
  551. sizeof(__be16));
  552. memcpy(skb_push(frame, ETH_ALEN), src, ETH_ALEN);
  553. memcpy(skb_push(frame, ETH_ALEN), dst, ETH_ALEN);
  554. }
  555. __skb_queue_tail(list, frame);
  556. }
  557. return;
  558. purge:
  559. __skb_queue_purge(list);
  560. out:
  561. dev_kfree_skb(skb);
  562. }
  563. EXPORT_SYMBOL(ieee80211_amsdu_to_8023s);
  564. /* Given a data frame determine the 802.1p/1d tag to use. */
  565. unsigned int cfg80211_classify8021d(struct sk_buff *skb)
  566. {
  567. unsigned int dscp;
  568. /* skb->priority values from 256->263 are magic values to
  569. * directly indicate a specific 802.1d priority. This is used
  570. * to allow 802.1d priority to be passed directly in from VLAN
  571. * tags, etc.
  572. */
  573. if (skb->priority >= 256 && skb->priority <= 263)
  574. return skb->priority - 256;
  575. switch (skb->protocol) {
  576. case htons(ETH_P_IP):
  577. dscp = ip_hdr(skb)->tos & 0xfc;
  578. break;
  579. default:
  580. return 0;
  581. }
  582. return dscp >> 5;
  583. }
  584. EXPORT_SYMBOL(cfg80211_classify8021d);
  585. const u8 *ieee80211_bss_get_ie(struct cfg80211_bss *bss, u8 ie)
  586. {
  587. u8 *end, *pos;
  588. pos = bss->information_elements;
  589. if (pos == NULL)
  590. return NULL;
  591. end = pos + bss->len_information_elements;
  592. while (pos + 1 < end) {
  593. if (pos + 2 + pos[1] > end)
  594. break;
  595. if (pos[0] == ie)
  596. return pos;
  597. pos += 2 + pos[1];
  598. }
  599. return NULL;
  600. }
  601. EXPORT_SYMBOL(ieee80211_bss_get_ie);
  602. void cfg80211_upload_connect_keys(struct wireless_dev *wdev)
  603. {
  604. struct cfg80211_registered_device *rdev = wiphy_to_dev(wdev->wiphy);
  605. struct net_device *dev = wdev->netdev;
  606. int i;
  607. if (!wdev->connect_keys)
  608. return;
  609. for (i = 0; i < 6; i++) {
  610. if (!wdev->connect_keys->params[i].cipher)
  611. continue;
  612. if (rdev->ops->add_key(wdev->wiphy, dev, i, false, NULL,
  613. &wdev->connect_keys->params[i])) {
  614. netdev_err(dev, "failed to set key %d\n", i);
  615. continue;
  616. }
  617. if (wdev->connect_keys->def == i)
  618. if (rdev->ops->set_default_key(wdev->wiphy, dev,
  619. i, true, true)) {
  620. netdev_err(dev, "failed to set defkey %d\n", i);
  621. continue;
  622. }
  623. if (wdev->connect_keys->defmgmt == i)
  624. if (rdev->ops->set_default_mgmt_key(wdev->wiphy, dev, i))
  625. netdev_err(dev, "failed to set mgtdef %d\n", i);
  626. }
  627. kfree(wdev->connect_keys);
  628. wdev->connect_keys = NULL;
  629. }
  630. static void cfg80211_process_wdev_events(struct wireless_dev *wdev)
  631. {
  632. struct cfg80211_event *ev;
  633. unsigned long flags;
  634. const u8 *bssid = NULL;
  635. spin_lock_irqsave(&wdev->event_lock, flags);
  636. while (!list_empty(&wdev->event_list)) {
  637. ev = list_first_entry(&wdev->event_list,
  638. struct cfg80211_event, list);
  639. list_del(&ev->list);
  640. spin_unlock_irqrestore(&wdev->event_lock, flags);
  641. wdev_lock(wdev);
  642. switch (ev->type) {
  643. case EVENT_CONNECT_RESULT:
  644. if (!is_zero_ether_addr(ev->cr.bssid))
  645. bssid = ev->cr.bssid;
  646. __cfg80211_connect_result(
  647. wdev->netdev, bssid,
  648. ev->cr.req_ie, ev->cr.req_ie_len,
  649. ev->cr.resp_ie, ev->cr.resp_ie_len,
  650. ev->cr.status,
  651. ev->cr.status == WLAN_STATUS_SUCCESS,
  652. NULL);
  653. break;
  654. case EVENT_ROAMED:
  655. __cfg80211_roamed(wdev, ev->rm.bss, ev->rm.req_ie,
  656. ev->rm.req_ie_len, ev->rm.resp_ie,
  657. ev->rm.resp_ie_len);
  658. break;
  659. case EVENT_DISCONNECTED:
  660. __cfg80211_disconnected(wdev->netdev,
  661. ev->dc.ie, ev->dc.ie_len,
  662. ev->dc.reason, true);
  663. break;
  664. case EVENT_IBSS_JOINED:
  665. __cfg80211_ibss_joined(wdev->netdev, ev->ij.bssid);
  666. break;
  667. }
  668. wdev_unlock(wdev);
  669. kfree(ev);
  670. spin_lock_irqsave(&wdev->event_lock, flags);
  671. }
  672. spin_unlock_irqrestore(&wdev->event_lock, flags);
  673. }
  674. void cfg80211_process_rdev_events(struct cfg80211_registered_device *rdev)
  675. {
  676. struct wireless_dev *wdev;
  677. ASSERT_RTNL();
  678. ASSERT_RDEV_LOCK(rdev);
  679. mutex_lock(&rdev->devlist_mtx);
  680. list_for_each_entry(wdev, &rdev->netdev_list, list)
  681. cfg80211_process_wdev_events(wdev);
  682. mutex_unlock(&rdev->devlist_mtx);
  683. }
  684. int cfg80211_change_iface(struct cfg80211_registered_device *rdev,
  685. struct net_device *dev, enum nl80211_iftype ntype,
  686. u32 *flags, struct vif_params *params)
  687. {
  688. int err;
  689. enum nl80211_iftype otype = dev->ieee80211_ptr->iftype;
  690. ASSERT_RDEV_LOCK(rdev);
  691. /* don't support changing VLANs, you just re-create them */
  692. if (otype == NL80211_IFTYPE_AP_VLAN)
  693. return -EOPNOTSUPP;
  694. if (!rdev->ops->change_virtual_intf ||
  695. !(rdev->wiphy.interface_modes & (1 << ntype)))
  696. return -EOPNOTSUPP;
  697. /* if it's part of a bridge, reject changing type to station/ibss */
  698. if ((dev->priv_flags & IFF_BRIDGE_PORT) &&
  699. (ntype == NL80211_IFTYPE_ADHOC ||
  700. ntype == NL80211_IFTYPE_STATION ||
  701. ntype == NL80211_IFTYPE_P2P_CLIENT))
  702. return -EBUSY;
  703. if (ntype != otype) {
  704. err = cfg80211_can_change_interface(rdev, dev->ieee80211_ptr,
  705. ntype);
  706. if (err)
  707. return err;
  708. dev->ieee80211_ptr->use_4addr = false;
  709. dev->ieee80211_ptr->mesh_id_up_len = 0;
  710. switch (otype) {
  711. case NL80211_IFTYPE_ADHOC:
  712. cfg80211_leave_ibss(rdev, dev, false);
  713. break;
  714. case NL80211_IFTYPE_STATION:
  715. case NL80211_IFTYPE_P2P_CLIENT:
  716. cfg80211_disconnect(rdev, dev,
  717. WLAN_REASON_DEAUTH_LEAVING, true);
  718. break;
  719. case NL80211_IFTYPE_MESH_POINT:
  720. /* mesh should be handled? */
  721. break;
  722. default:
  723. break;
  724. }
  725. cfg80211_process_rdev_events(rdev);
  726. }
  727. err = rdev->ops->change_virtual_intf(&rdev->wiphy, dev,
  728. ntype, flags, params);
  729. WARN_ON(!err && dev->ieee80211_ptr->iftype != ntype);
  730. if (!err && params && params->use_4addr != -1)
  731. dev->ieee80211_ptr->use_4addr = params->use_4addr;
  732. if (!err) {
  733. dev->priv_flags &= ~IFF_DONT_BRIDGE;
  734. switch (ntype) {
  735. case NL80211_IFTYPE_STATION:
  736. if (dev->ieee80211_ptr->use_4addr)
  737. break;
  738. /* fall through */
  739. case NL80211_IFTYPE_P2P_CLIENT:
  740. case NL80211_IFTYPE_ADHOC:
  741. dev->priv_flags |= IFF_DONT_BRIDGE;
  742. break;
  743. case NL80211_IFTYPE_P2P_GO:
  744. case NL80211_IFTYPE_AP:
  745. case NL80211_IFTYPE_AP_VLAN:
  746. case NL80211_IFTYPE_WDS:
  747. case NL80211_IFTYPE_MESH_POINT:
  748. /* bridging OK */
  749. break;
  750. case NL80211_IFTYPE_MONITOR:
  751. /* monitor can't bridge anyway */
  752. break;
  753. case NL80211_IFTYPE_UNSPECIFIED:
  754. case NUM_NL80211_IFTYPES:
  755. /* not happening */
  756. break;
  757. }
  758. }
  759. return err;
  760. }
  761. u16 cfg80211_calculate_bitrate(struct rate_info *rate)
  762. {
  763. int modulation, streams, bitrate;
  764. if (!(rate->flags & RATE_INFO_FLAGS_MCS))
  765. return rate->legacy;
  766. /* the formula below does only work for MCS values smaller than 32 */
  767. if (rate->mcs >= 32)
  768. return 0;
  769. modulation = rate->mcs & 7;
  770. streams = (rate->mcs >> 3) + 1;
  771. bitrate = (rate->flags & RATE_INFO_FLAGS_40_MHZ_WIDTH) ?
  772. 13500000 : 6500000;
  773. if (modulation < 4)
  774. bitrate *= (modulation + 1);
  775. else if (modulation == 4)
  776. bitrate *= (modulation + 2);
  777. else
  778. bitrate *= (modulation + 3);
  779. bitrate *= streams;
  780. if (rate->flags & RATE_INFO_FLAGS_SHORT_GI)
  781. bitrate = (bitrate / 9) * 10;
  782. /* do NOT round down here */
  783. return (bitrate + 50000) / 100000;
  784. }
  785. int cfg80211_validate_beacon_int(struct cfg80211_registered_device *rdev,
  786. u32 beacon_int)
  787. {
  788. struct wireless_dev *wdev;
  789. int res = 0;
  790. if (!beacon_int)
  791. return -EINVAL;
  792. mutex_lock(&rdev->devlist_mtx);
  793. list_for_each_entry(wdev, &rdev->netdev_list, list) {
  794. if (!wdev->beacon_interval)
  795. continue;
  796. if (wdev->beacon_interval != beacon_int) {
  797. res = -EINVAL;
  798. break;
  799. }
  800. }
  801. mutex_unlock(&rdev->devlist_mtx);
  802. return res;
  803. }
  804. int cfg80211_can_change_interface(struct cfg80211_registered_device *rdev,
  805. struct wireless_dev *wdev,
  806. enum nl80211_iftype iftype)
  807. {
  808. struct wireless_dev *wdev_iter;
  809. int num[NUM_NL80211_IFTYPES];
  810. int total = 1;
  811. int i, j;
  812. ASSERT_RTNL();
  813. /* Always allow software iftypes */
  814. if (rdev->wiphy.software_iftypes & BIT(iftype))
  815. return 0;
  816. /*
  817. * Drivers will gradually all set this flag, until all
  818. * have it we only enforce for those that set it.
  819. */
  820. if (!(rdev->wiphy.flags & WIPHY_FLAG_ENFORCE_COMBINATIONS))
  821. return 0;
  822. memset(num, 0, sizeof(num));
  823. num[iftype] = 1;
  824. mutex_lock(&rdev->devlist_mtx);
  825. list_for_each_entry(wdev_iter, &rdev->netdev_list, list) {
  826. if (wdev_iter == wdev)
  827. continue;
  828. if (!netif_running(wdev_iter->netdev))
  829. continue;
  830. if (rdev->wiphy.software_iftypes & BIT(wdev_iter->iftype))
  831. continue;
  832. num[wdev_iter->iftype]++;
  833. total++;
  834. }
  835. mutex_unlock(&rdev->devlist_mtx);
  836. for (i = 0; i < rdev->wiphy.n_iface_combinations; i++) {
  837. const struct ieee80211_iface_combination *c;
  838. struct ieee80211_iface_limit *limits;
  839. c = &rdev->wiphy.iface_combinations[i];
  840. limits = kmemdup(c->limits, sizeof(limits[0]) * c->n_limits,
  841. GFP_KERNEL);
  842. if (!limits)
  843. return -ENOMEM;
  844. if (total > c->max_interfaces)
  845. goto cont;
  846. for (iftype = 0; iftype < NUM_NL80211_IFTYPES; iftype++) {
  847. if (rdev->wiphy.software_iftypes & BIT(iftype))
  848. continue;
  849. for (j = 0; j < c->n_limits; j++) {
  850. if (!(limits[j].types & iftype))
  851. continue;
  852. if (limits[j].max < num[iftype])
  853. goto cont;
  854. limits[j].max -= num[iftype];
  855. }
  856. }
  857. /* yay, it fits */
  858. kfree(limits);
  859. return 0;
  860. cont:
  861. kfree(limits);
  862. }
  863. return -EBUSY;
  864. }
  865. int ieee80211_get_ratemask(struct ieee80211_supported_band *sband,
  866. const u8 *rates, unsigned int n_rates,
  867. u32 *mask)
  868. {
  869. int i, j;
  870. if (!sband)
  871. return -EINVAL;
  872. if (n_rates == 0 || n_rates > NL80211_MAX_SUPP_RATES)
  873. return -EINVAL;
  874. *mask = 0;
  875. for (i = 0; i < n_rates; i++) {
  876. int rate = (rates[i] & 0x7f) * 5;
  877. bool found = false;
  878. for (j = 0; j < sband->n_bitrates; j++) {
  879. if (sband->bitrates[j].bitrate == rate) {
  880. found = true;
  881. *mask |= BIT(j);
  882. break;
  883. }
  884. }
  885. if (!found)
  886. return -EINVAL;
  887. }
  888. /*
  889. * mask must have at least one bit set here since we
  890. * didn't accept a 0-length rates array nor allowed
  891. * entries in the array that didn't exist
  892. */
  893. return 0;
  894. }
  895. /* See IEEE 802.1H for LLC/SNAP encapsulation/decapsulation */
  896. /* Ethernet-II snap header (RFC1042 for most EtherTypes) */
  897. const unsigned char rfc1042_header[] __aligned(2) =
  898. { 0xaa, 0xaa, 0x03, 0x00, 0x00, 0x00 };
  899. EXPORT_SYMBOL(rfc1042_header);
  900. /* Bridge-Tunnel header (for EtherTypes ETH_P_AARP and ETH_P_IPX) */
  901. const unsigned char bridge_tunnel_header[] __aligned(2) =
  902. { 0xaa, 0xaa, 0x03, 0x00, 0x00, 0xf8 };
  903. EXPORT_SYMBOL(bridge_tunnel_header);