net/wireless/util.c

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