net/wireless/util.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 /*
   3  * Wireless utility functions
   4  *
   5  * Copyright 2007-2009  Johannes Berg <johannes@sipsolutions.net>
   6  * Copyright 2013-2014  Intel Mobile Communications GmbH
   7  * Copyright 2017       Intel Deutschland GmbH
   8  * Copyright (C) 2018-2023 Intel Corporation
   9  */
  10 #include <linux/export.h>
  11 #include <linux/bitops.h>
  12 #include <linux/etherdevice.h>
  13 #include <linux/slab.h>
  14 #include <linux/ieee80211.h>
  15 #include <net/cfg80211.h>
  16 #include <net/ip.h>
  17 #include <net/dsfield.h>
  18 #include <linux/if_vlan.h>
  19 #include <linux/mpls.h>
  20 #include <linux/gcd.h>
  21 #include <linux/bitfield.h>
  22 #include <linux/nospec.h>
  23 #include "core.h"
  24 #include "rdev-ops.h"
  25
  26
  27 const struct ieee80211_rate *
  28 ieee80211_get_response_rate(struct ieee80211_supported_band *sband,
  29                             u32 basic_rates, int bitrate)
  30 {
  31         struct ieee80211_rate *result = &sband->bitrates[0];
  32         int i;
  33
  34         for (i = 0; i < sband->n_bitrates; i++) {
  35                 if (!(basic_rates & BIT(i)))
  36                         continue;
  37                 if (sband->bitrates[i].bitrate > bitrate)
  38                         continue;
  39                 result = &sband->bitrates[i];
  40         }
  41
  42         return result;
  43 }
  44 EXPORT_SYMBOL(ieee80211_get_response_rate);
  45
  46 u32 ieee80211_mandatory_rates(struct ieee80211_supported_band *sband)
  47 {
  48         struct ieee80211_rate *bitrates;
  49         u32 mandatory_rates = 0;
  50         enum ieee80211_rate_flags mandatory_flag;
  51         int i;
  52
  53         if (WARN_ON(!sband))
  54                 return 1;
  55
  56         if (sband->band == NL80211_BAND_2GHZ)
  57                 mandatory_flag = IEEE80211_RATE_MANDATORY_B;
  58         else
  59                 mandatory_flag = IEEE80211_RATE_MANDATORY_A;
  60
  61         bitrates = sband->bitrates;
  62         for (i = 0; i < sband->n_bitrates; i++)
  63                 if (bitrates[i].flags & mandatory_flag)
  64                         mandatory_rates |= BIT(i);
  65         return mandatory_rates;
  66 }
  67 EXPORT_SYMBOL(ieee80211_mandatory_rates);
  68
  69 u32 ieee80211_channel_to_freq_khz(int chan, enum nl80211_band band)
  70 {
  71         /* see 802.11 17.3.8.3.2 and Annex J
  72          * there are overlapping channel numbers in 5GHz and 2GHz bands */
  73         if (chan <= 0)
  74                 return 0; /* not supported */
  75         switch (band) {
  76         case NL80211_BAND_2GHZ:
  77         case NL80211_BAND_LC:
  78                 if (chan == 14)
  79                         return MHZ_TO_KHZ(2484);
  80                 else if (chan < 14)
  81                         return MHZ_TO_KHZ(2407 + chan * 5);
  82                 break;
  83         case NL80211_BAND_5GHZ:
  84                 if (chan >= 182 && chan <= 196)
  85                         return MHZ_TO_KHZ(4000 + chan * 5);
  86                 else
  87                         return MHZ_TO_KHZ(5000 + chan * 5);
  88                 break;
  89         case NL80211_BAND_6GHZ:
  90                 /* see 802.11ax D6.1 27.3.23.2 */
  91                 if (chan == 2)
  92                         return MHZ_TO_KHZ(5935);
  93                 if (chan <= 233)
  94                         return MHZ_TO_KHZ(5950 + chan * 5);
  95                 break;
  96         case NL80211_BAND_60GHZ:
  97                 if (chan < 7)
  98                         return MHZ_TO_KHZ(56160 + chan * 2160);
  99                 break;
 100         case NL80211_BAND_S1GHZ:
 101                 return 902000 + chan * 500;
 102         default:
 103                 ;
 104         }
 105         return 0; /* not supported */
 106 }
 107 EXPORT_SYMBOL(ieee80211_channel_to_freq_khz);
 108
 109 enum nl80211_chan_width
 110 ieee80211_s1g_channel_width(const struct ieee80211_channel *chan)
 111 {
 112         if (WARN_ON(!chan || chan->band != NL80211_BAND_S1GHZ))
 113                 return NL80211_CHAN_WIDTH_20_NOHT;
 114
 115         /*S1G defines a single allowed channel width per channel.
 116          * Extract that width here.
 117          */
 118         if (chan->flags & IEEE80211_CHAN_1MHZ)
 119                 return NL80211_CHAN_WIDTH_1;
 120         else if (chan->flags & IEEE80211_CHAN_2MHZ)
 121                 return NL80211_CHAN_WIDTH_2;
 122         else if (chan->flags & IEEE80211_CHAN_4MHZ)
 123                 return NL80211_CHAN_WIDTH_4;
 124         else if (chan->flags & IEEE80211_CHAN_8MHZ)
 125                 return NL80211_CHAN_WIDTH_8;
 126         else if (chan->flags & IEEE80211_CHAN_16MHZ)
 127                 return NL80211_CHAN_WIDTH_16;
 128
 129         pr_err("unknown channel width for channel at %dKHz?\n",
 130                ieee80211_channel_to_khz(chan));
 131
 132         return NL80211_CHAN_WIDTH_1;
 133 }
 134 EXPORT_SYMBOL(ieee80211_s1g_channel_width);
 135
 136 int ieee80211_freq_khz_to_channel(u32 freq)
 137 {
 138         /* TODO: just handle MHz for now */
 139         freq = KHZ_TO_MHZ(freq);
 140
 141         /* see 802.11 17.3.8.3.2 and Annex J */
 142         if (freq == 2484)
 143                 return 14;
 144         else if (freq < 2484)
 145                 return (freq - 2407) / 5;
 146         else if (freq >= 4910 && freq <= 4980)
 147                 return (freq - 4000) / 5;
 148         else if (freq < 5925)
 149                 return (freq - 5000) / 5;
 150         else if (freq == 5935)
 151                 return 2;
 152         else if (freq <= 45000) /* DMG band lower limit */
 153                 /* see 802.11ax D6.1 27.3.22.2 */
 154                 return (freq - 5950) / 5;
 155         else if (freq >= 58320 && freq <= 70200)
 156                 return (freq - 56160) / 2160;
 157         else
 158                 return 0;
 159 }
 160 EXPORT_SYMBOL(ieee80211_freq_khz_to_channel);
 161
 162 struct ieee80211_channel *ieee80211_get_channel_khz(struct wiphy *wiphy,
 163                                                     u32 freq)
 164 {
 165         enum nl80211_band band;
 166         struct ieee80211_supported_band *sband;
 167         int i;
 168
 169         for (band = 0; band < NUM_NL80211_BANDS; band++) {
 170                 sband = wiphy->bands[band];
 171
 172                 if (!sband)
 173                         continue;
 174
 175                 for (i = 0; i < sband->n_channels; i++) {
 176                         struct ieee80211_channel *chan = &sband->channels[i];
 177
 178                         if (ieee80211_channel_to_khz(chan) == freq)
 179                                 return chan;
 180                 }
 181         }
 182
 183         return NULL;
 184 }
 185 EXPORT_SYMBOL(ieee80211_get_channel_khz);
 186
 187 static void set_mandatory_flags_band(struct ieee80211_supported_band *sband)
 188 {
 189         int i, want;
 190
 191         switch (sband->band) {
 192         case NL80211_BAND_5GHZ:
 193         case NL80211_BAND_6GHZ:
 194                 want = 3;
 195                 for (i = 0; i < sband->n_bitrates; i++) {
 196                         if (sband->bitrates[i].bitrate == 60 ||
 197                             sband->bitrates[i].bitrate == 120 ||
 198                             sband->bitrates[i].bitrate == 240) {
 199                                 sband->bitrates[i].flags |=
 200                                         IEEE80211_RATE_MANDATORY_A;
 201                                 want--;
 202                         }
 203                 }
 204                 WARN_ON(want);
 205                 break;
 206         case NL80211_BAND_2GHZ:
 207         case NL80211_BAND_LC:
 208                 want = 7;
 209                 for (i = 0; i < sband->n_bitrates; i++) {
 210                         switch (sband->bitrates[i].bitrate) {
 211                         case 10:
 212                         case 20:
 213                         case 55:
 214                         case 110:
 215                                 sband->bitrates[i].flags |=
 216                                         IEEE80211_RATE_MANDATORY_B |
 217                                         IEEE80211_RATE_MANDATORY_G;
 218                                 want--;
 219                                 break;
 220                         case 60:
 221                         case 120:
 222                         case 240:
 223                                 sband->bitrates[i].flags |=
 224                                         IEEE80211_RATE_MANDATORY_G;
 225                                 want--;
 226                                 fallthrough;
 227                         default:
 228                                 sband->bitrates[i].flags |=
 229                                         IEEE80211_RATE_ERP_G;
 230                                 break;
 231                         }
 232                 }
 233                 WARN_ON(want != 0 && want != 3);
 234                 break;
 235         case NL80211_BAND_60GHZ:
 236                 /* check for mandatory HT MCS 1..4 */
 237                 WARN_ON(!sband->ht_cap.ht_supported);
 238                 WARN_ON((sband->ht_cap.mcs.rx_mask[0] & 0x1e) != 0x1e);
 239                 break;
 240         case NL80211_BAND_S1GHZ:
 241                 /* Figure 9-589bd: 3 means unsupported, so != 3 means at least
 242                  * mandatory is ok.
 243                  */
 244                 WARN_ON((sband->s1g_cap.nss_mcs[0] & 0x3) == 0x3);
 245                 break;
 246         case NUM_NL80211_BANDS:
 247         default:
 248                 WARN_ON(1);
 249                 break;
 250         }
 251 }
 252
 253 void ieee80211_set_bitrate_flags(struct wiphy *wiphy)
 254 {
 255         enum nl80211_band band;
 256
 257         for (band = 0; band < NUM_NL80211_BANDS; band++)
 258                 if (wiphy->bands[band])
 259                         set_mandatory_flags_band(wiphy->bands[band]);
 260 }
 261
 262 bool cfg80211_supported_cipher_suite(struct wiphy *wiphy, u32 cipher)
 263 {
 264         int i;
 265         for (i = 0; i < wiphy->n_cipher_suites; i++)
 266                 if (cipher == wiphy->cipher_suites[i])
 267                         return true;
 268         return false;
 269 }
 270
 271 static bool
 272 cfg80211_igtk_cipher_supported(struct cfg80211_registered_device *rdev)
 273 {
 274         struct wiphy *wiphy = &rdev->wiphy;
 275         int i;
 276
 277         for (i = 0; i < wiphy->n_cipher_suites; i++) {
 278                 switch (wiphy->cipher_suites[i]) {
 279                 case WLAN_CIPHER_SUITE_AES_CMAC:
 280                 case WLAN_CIPHER_SUITE_BIP_CMAC_256:
 281                 case WLAN_CIPHER_SUITE_BIP_GMAC_128:
 282                 case WLAN_CIPHER_SUITE_BIP_GMAC_256:
 283                         return true;
 284                 }
 285         }
 286
 287         return false;
 288 }
 289
 290 bool cfg80211_valid_key_idx(struct cfg80211_registered_device *rdev,
 291                             int key_idx, bool pairwise)
 292 {
 293         int max_key_idx;
 294
 295         if (pairwise)
 296                 max_key_idx = 3;
 297         else if (wiphy_ext_feature_isset(&rdev->wiphy,
 298                                          NL80211_EXT_FEATURE_BEACON_PROTECTION) ||
 299                  wiphy_ext_feature_isset(&rdev->wiphy,
 300                                          NL80211_EXT_FEATURE_BEACON_PROTECTION_CLIENT))
 301                 max_key_idx = 7;
 302         else if (cfg80211_igtk_cipher_supported(rdev))
 303                 max_key_idx = 5;
 304         else
 305                 max_key_idx = 3;
 306
 307         if (key_idx < 0 || key_idx > max_key_idx)
 308                 return false;
 309
 310         return true;
 311 }
 312
 313 int cfg80211_validate_key_settings(struct cfg80211_registered_device *rdev,
 314                                    struct key_params *params, int key_idx,
 315                                    bool pairwise, const u8 *mac_addr)
 316 {
 317         if (!cfg80211_valid_key_idx(rdev, key_idx, pairwise))
 318                 return -EINVAL;
 319
 320         if (!pairwise && mac_addr && !(rdev->wiphy.flags & WIPHY_FLAG_IBSS_RSN))
 321                 return -EINVAL;
 322
 323         if (pairwise && !mac_addr)
 324                 return -EINVAL;
 325
 326         switch (params->cipher) {
 327         case WLAN_CIPHER_SUITE_TKIP:
 328                 /* Extended Key ID can only be used with CCMP/GCMP ciphers */
 329                 if ((pairwise && key_idx) ||
 330                     params->mode != NL80211_KEY_RX_TX)
 331                         return -EINVAL;
 332                 break;
 333         case WLAN_CIPHER_SUITE_CCMP:
 334         case WLAN_CIPHER_SUITE_CCMP_256:
 335         case WLAN_CIPHER_SUITE_GCMP:
 336         case WLAN_CIPHER_SUITE_GCMP_256:
 337                 /* IEEE802.11-2016 allows only 0 and - when supporting
 338                  * Extended Key ID - 1 as index for pairwise keys.
 339                  * @NL80211_KEY_NO_TX is only allowed for pairwise keys when
 340                  * the driver supports Extended Key ID.
 341                  * @NL80211_KEY_SET_TX can't be set when installing and
 342                  * validating a key.
 343                  */
 344                 if ((params->mode == NL80211_KEY_NO_TX && !pairwise) ||
 345                     params->mode == NL80211_KEY_SET_TX)
 346                         return -EINVAL;
 347                 if (wiphy_ext_feature_isset(&rdev->wiphy,
 348                                             NL80211_EXT_FEATURE_EXT_KEY_ID)) {
 349                         if (pairwise && (key_idx < 0 || key_idx > 1))
 350                                 return -EINVAL;
 351                 } else if (pairwise && key_idx) {
 352                         return -EINVAL;
 353                 }
 354                 break;
 355         case WLAN_CIPHER_SUITE_AES_CMAC:
 356         case WLAN_CIPHER_SUITE_BIP_CMAC_256:
 357         case WLAN_CIPHER_SUITE_BIP_GMAC_128:
 358         case WLAN_CIPHER_SUITE_BIP_GMAC_256:
 359                 /* Disallow BIP (group-only) cipher as pairwise cipher */
 360                 if (pairwise)
 361                         return -EINVAL;
 362                 if (key_idx < 4)
 363                         return -EINVAL;
 364                 break;
 365         case WLAN_CIPHER_SUITE_WEP40:
 366         case WLAN_CIPHER_SUITE_WEP104:
 367                 if (key_idx > 3)
 368                         return -EINVAL;
 369                 break;
 370         default:
 371                 break;
 372         }
 373
 374         switch (params->cipher) {
 375         case WLAN_CIPHER_SUITE_WEP40:
 376                 if (params->key_len != WLAN_KEY_LEN_WEP40)
 377                         return -EINVAL;
 378                 break;
 379         case WLAN_CIPHER_SUITE_TKIP:
 380                 if (params->key_len != WLAN_KEY_LEN_TKIP)
 381                         return -EINVAL;
 382                 break;
 383         case WLAN_CIPHER_SUITE_CCMP:
 384                 if (params->key_len != WLAN_KEY_LEN_CCMP)
 385                         return -EINVAL;
 386                 break;
 387         case WLAN_CIPHER_SUITE_CCMP_256:
 388                 if (params->key_len != WLAN_KEY_LEN_CCMP_256)
 389                         return -EINVAL;
 390                 break;
 391         case WLAN_CIPHER_SUITE_GCMP:
 392                 if (params->key_len != WLAN_KEY_LEN_GCMP)
 393                         return -EINVAL;
 394                 break;
 395         case WLAN_CIPHER_SUITE_GCMP_256:
 396                 if (params->key_len != WLAN_KEY_LEN_GCMP_256)
 397                         return -EINVAL;
 398                 break;
 399         case WLAN_CIPHER_SUITE_WEP104:
 400                 if (params->key_len != WLAN_KEY_LEN_WEP104)
 401                         return -EINVAL;
 402                 break;
 403         case WLAN_CIPHER_SUITE_AES_CMAC:
 404                 if (params->key_len != WLAN_KEY_LEN_AES_CMAC)
 405                         return -EINVAL;
 406                 break;
 407         case WLAN_CIPHER_SUITE_BIP_CMAC_256:
 408                 if (params->key_len != WLAN_KEY_LEN_BIP_CMAC_256)
 409                         return -EINVAL;
 410                 break;
 411         case WLAN_CIPHER_SUITE_BIP_GMAC_128:
 412                 if (params->key_len != WLAN_KEY_LEN_BIP_GMAC_128)
 413                         return -EINVAL;
 414                 break;
 415         case WLAN_CIPHER_SUITE_BIP_GMAC_256:
 416                 if (params->key_len != WLAN_KEY_LEN_BIP_GMAC_256)
 417                         return -EINVAL;
 418                 break;
 419         default:
 420                 /*
 421                  * We don't know anything about this algorithm,
 422                  * allow using it -- but the driver must check
 423                  * all parameters! We still check below whether
 424                  * or not the driver supports this algorithm,
 425                  * of course.
 426                  */
 427                 break;
 428         }
 429
 430         if (params->seq) {
 431                 switch (params->cipher) {
 432                 case WLAN_CIPHER_SUITE_WEP40:
 433                 case WLAN_CIPHER_SUITE_WEP104:
 434                         /* These ciphers do not use key sequence */
 435                         return -EINVAL;
 436                 case WLAN_CIPHER_SUITE_TKIP:
 437                 case WLAN_CIPHER_SUITE_CCMP:
 438                 case WLAN_CIPHER_SUITE_CCMP_256:
 439                 case WLAN_CIPHER_SUITE_GCMP:
 440                 case WLAN_CIPHER_SUITE_GCMP_256:
 441                 case WLAN_CIPHER_SUITE_AES_CMAC:
 442                 case WLAN_CIPHER_SUITE_BIP_CMAC_256:
 443                 case WLAN_CIPHER_SUITE_BIP_GMAC_128:
 444                 case WLAN_CIPHER_SUITE_BIP_GMAC_256:
 445                         if (params->seq_len != 6)
 446                                 return -EINVAL;
 447                         break;
 448                 }
 449         }
 450
 451         if (!cfg80211_supported_cipher_suite(&rdev->wiphy, params->cipher))
 452                 return -EINVAL;
 453
 454         return 0;
 455 }
 456
 457 unsigned int __attribute_const__ ieee80211_hdrlen(__le16 fc)
 458 {
 459         unsigned int hdrlen = 24;
 460
 461         if (ieee80211_is_ext(fc)) {
 462                 hdrlen = 4;
 463                 goto out;
 464         }
 465
 466         if (ieee80211_is_data(fc)) {
 467                 if (ieee80211_has_a4(fc))
 468                         hdrlen = 30;
 469                 if (ieee80211_is_data_qos(fc)) {
 470                         hdrlen += IEEE80211_QOS_CTL_LEN;
 471                         if (ieee80211_has_order(fc))
 472                                 hdrlen += IEEE80211_HT_CTL_LEN;
 473                 }
 474                 goto out;
 475         }
 476
 477         if (ieee80211_is_mgmt(fc)) {
 478                 if (ieee80211_has_order(fc))
 479                         hdrlen += IEEE80211_HT_CTL_LEN;
 480                 goto out;
 481         }
 482
 483         if (ieee80211_is_ctl(fc)) {
 484                 /*
 485                  * ACK and CTS are 10 bytes, all others 16. To see how
 486                  * to get this condition consider
 487                  *   subtype mask:   0b0000000011110000 (0x00F0)
 488                  *   ACK subtype:    0b0000000011010000 (0x00D0)
 489                  *   CTS subtype:    0b0000000011000000 (0x00C0)
 490                  *   bits that matter:         ^^^      (0x00E0)
 491                  *   value of those: 0b0000000011000000 (0x00C0)
 492                  */
 493                 if ((fc & cpu_to_le16(0x00E0)) == cpu_to_le16(0x00C0))
 494                         hdrlen = 10;
 495                 else
 496                         hdrlen = 16;
 497         }
 498 out:
 499         return hdrlen;
 500 }
 501 EXPORT_SYMBOL(ieee80211_hdrlen);
 502
 503 unsigned int ieee80211_get_hdrlen_from_skb(const struct sk_buff *skb)
 504 {
 505         const struct ieee80211_hdr *hdr =
 506                         (const struct ieee80211_hdr *)skb->data;
 507         unsigned int hdrlen;
 508
 509         if (unlikely(skb->len < 10))
 510                 return 0;
 511         hdrlen = ieee80211_hdrlen(hdr->frame_control);
 512         if (unlikely(hdrlen > skb->len))
 513                 return 0;
 514         return hdrlen;
 515 }
 516 EXPORT_SYMBOL(ieee80211_get_hdrlen_from_skb);
 517
 518 static unsigned int __ieee80211_get_mesh_hdrlen(u8 flags)
 519 {
 520         int ae = flags & MESH_FLAGS_AE;
 521         /* 802.11-2012, 8.2.4.7.3 */
 522         switch (ae) {
 523         default:
 524         case 0:
 525                 return 6;
 526         case MESH_FLAGS_AE_A4:
 527                 return 12;
 528         case MESH_FLAGS_AE_A5_A6:
 529                 return 18;
 530         }
 531 }
 532
 533 unsigned int ieee80211_get_mesh_hdrlen(struct ieee80211s_hdr *meshhdr)
 534 {
 535         return __ieee80211_get_mesh_hdrlen(meshhdr->flags);
 536 }
 537 EXPORT_SYMBOL(ieee80211_get_mesh_hdrlen);
 538
 539 bool ieee80211_get_8023_tunnel_proto(const void *hdr, __be16 *proto)
 540 {
 541         const __be16 *hdr_proto = hdr + ETH_ALEN;
 542
 543         if (!(ether_addr_equal(hdr, rfc1042_header) &&
 544               *hdr_proto != htons(ETH_P_AARP) &&
 545               *hdr_proto != htons(ETH_P_IPX)) &&
 546             !ether_addr_equal(hdr, bridge_tunnel_header))
 547                 return false;
 548
 549         *proto = *hdr_proto;
 550
 551         return true;
 552 }
 553 EXPORT_SYMBOL(ieee80211_get_8023_tunnel_proto);
 554
 555 int ieee80211_strip_8023_mesh_hdr(struct sk_buff *skb)
 556 {
 557         const void *mesh_addr;
 558         struct {
 559                 struct ethhdr eth;
 560                 u8 flags;
 561         } payload;
 562         int hdrlen;
 563         int ret;
 564
 565         ret = skb_copy_bits(skb, 0, &payload, sizeof(payload));
 566         if (ret)
 567                 return ret;
 568
 569         hdrlen = sizeof(payload.eth) + __ieee80211_get_mesh_hdrlen(payload.flags);
 570
 571         if (likely(pskb_may_pull(skb, hdrlen + 8) &&
 572                    ieee80211_get_8023_tunnel_proto(skb->data + hdrlen,
 573                                                    &payload.eth.h_proto)))
 574                 hdrlen += ETH_ALEN + 2;
 575         else if (!pskb_may_pull(skb, hdrlen))
 576                 return -EINVAL;
 577         else
 578                 payload.eth.h_proto = htons(skb->len - hdrlen);
 579
 580         mesh_addr = skb->data + sizeof(payload.eth) + ETH_ALEN;
 581         switch (payload.flags & MESH_FLAGS_AE) {
 582         case MESH_FLAGS_AE_A4:
 583                 memcpy(&payload.eth.h_source, mesh_addr, ETH_ALEN);
 584                 break;
 585         case MESH_FLAGS_AE_A5_A6:
 586                 memcpy(&payload.eth, mesh_addr, 2 * ETH_ALEN);
 587                 break;
 588         default:
 589                 break;
 590         }
 591
 592         pskb_pull(skb, hdrlen - sizeof(payload.eth));
 593         memcpy(skb->data, &payload.eth, sizeof(payload.eth));
 594
 595         return 0;
 596 }
 597 EXPORT_SYMBOL(ieee80211_strip_8023_mesh_hdr);
 598
 599 int ieee80211_data_to_8023_exthdr(struct sk_buff *skb, struct ethhdr *ehdr,
 600                                   const u8 *addr, enum nl80211_iftype iftype,
 601                                   u8 data_offset, bool is_amsdu)
 602 {
 603         struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
 604         struct {
 605                 u8 hdr[ETH_ALEN] __aligned(2);
 606                 __be16 proto;
 607         } payload;
 608         struct ethhdr tmp;
 609         u16 hdrlen;
 610
 611         if (unlikely(!ieee80211_is_data_present(hdr->frame_control)))
 612                 return -1;
 613
 614         hdrlen = ieee80211_hdrlen(hdr->frame_control) + data_offset;
 615         if (skb->len < hdrlen)
 616                 return -1;
 617
 618         /* convert IEEE 802.11 header + possible LLC headers into Ethernet
 619          * header
 620          * IEEE 802.11 address fields:
 621          * ToDS FromDS Addr1 Addr2 Addr3 Addr4
 622          *   0     0   DA    SA    BSSID n/a
 623          *   0     1   DA    BSSID SA    n/a
 624          *   1     0   BSSID SA    DA    n/a
 625          *   1     1   RA    TA    DA    SA
 626          */
 627         memcpy(tmp.h_dest, ieee80211_get_DA(hdr), ETH_ALEN);
 628         memcpy(tmp.h_source, ieee80211_get_SA(hdr), ETH_ALEN);
 629
 630         switch (hdr->frame_control &
 631                 cpu_to_le16(IEEE80211_FCTL_TODS | IEEE80211_FCTL_FROMDS)) {
 632         case cpu_to_le16(IEEE80211_FCTL_TODS):
 633                 if (unlikely(iftype != NL80211_IFTYPE_AP &&
 634                              iftype != NL80211_IFTYPE_AP_VLAN &&
 635                              iftype != NL80211_IFTYPE_P2P_GO))
 636                         return -1;
 637                 break;
 638         case cpu_to_le16(IEEE80211_FCTL_TODS | IEEE80211_FCTL_FROMDS):
 639                 if (unlikely(iftype != NL80211_IFTYPE_MESH_POINT &&
 640                              iftype != NL80211_IFTYPE_AP_VLAN &&
 641                              iftype != NL80211_IFTYPE_STATION))
 642                         return -1;
 643                 break;
 644         case cpu_to_le16(IEEE80211_FCTL_FROMDS):
 645                 if ((iftype != NL80211_IFTYPE_STATION &&
 646                      iftype != NL80211_IFTYPE_P2P_CLIENT &&
 647                      iftype != NL80211_IFTYPE_MESH_POINT) ||
 648                     (is_multicast_ether_addr(tmp.h_dest) &&
 649                      ether_addr_equal(tmp.h_source, addr)))
 650                         return -1;
 651                 break;
 652         case cpu_to_le16(0):
 653                 if (iftype != NL80211_IFTYPE_ADHOC &&
 654                     iftype != NL80211_IFTYPE_STATION &&
 655                     iftype != NL80211_IFTYPE_OCB)
 656                                 return -1;
 657                 break;
 658         }
 659
 660         if (likely(!is_amsdu && iftype != NL80211_IFTYPE_MESH_POINT &&
 661                    skb_copy_bits(skb, hdrlen, &payload, sizeof(payload)) == 0 &&
 662                    ieee80211_get_8023_tunnel_proto(&payload, &tmp.h_proto))) {
 663                 /* remove RFC1042 or Bridge-Tunnel encapsulation */
 664                 hdrlen += ETH_ALEN + 2;
 665                 skb_postpull_rcsum(skb, &payload, ETH_ALEN + 2);
 666         } else {
 667                 tmp.h_proto = htons(skb->len - hdrlen);
 668         }
 669
 670         pskb_pull(skb, hdrlen);
 671
 672         if (!ehdr)
 673                 ehdr = skb_push(skb, sizeof(struct ethhdr));
 674         memcpy(ehdr, &tmp, sizeof(tmp));
 675
 676         return 0;
 677 }
 678 EXPORT_SYMBOL(ieee80211_data_to_8023_exthdr);
 679
 680 static void
 681 __frame_add_frag(struct sk_buff *skb, struct page *page,
 682                  void *ptr, int len, int size)
 683 {
 684         struct skb_shared_info *sh = skb_shinfo(skb);
 685         int page_offset;
 686
 687         get_page(page);
 688         page_offset = ptr - page_address(page);
 689         skb_add_rx_frag(skb, sh->nr_frags, page, page_offset, len, size);
 690 }
 691
 692 static void
 693 __ieee80211_amsdu_copy_frag(struct sk_buff *skb, struct sk_buff *frame,
 694                             int offset, int len)
 695 {
 696         struct skb_shared_info *sh = skb_shinfo(skb);
 697         const skb_frag_t *frag = &sh->frags[0];
 698         struct page *frag_page;
 699         void *frag_ptr;
 700         int frag_len, frag_size;
 701         int head_size = skb->len - skb->data_len;
 702         int cur_len;
 703
 704         frag_page = virt_to_head_page(skb->head);
 705         frag_ptr = skb->data;
 706         frag_size = head_size;
 707
 708         while (offset >= frag_size) {
 709                 offset -= frag_size;
 710                 frag_page = skb_frag_page(frag);
 711                 frag_ptr = skb_frag_address(frag);
 712                 frag_size = skb_frag_size(frag);
 713                 frag++;
 714         }
 715
 716         frag_ptr += offset;
 717         frag_len = frag_size - offset;
 718
 719         cur_len = min(len, frag_len);
 720
 721         __frame_add_frag(frame, frag_page, frag_ptr, cur_len, frag_size);
 722         len -= cur_len;
 723
 724         while (len > 0) {
 725                 frag_len = skb_frag_size(frag);
 726                 cur_len = min(len, frag_len);
 727                 __frame_add_frag(frame, skb_frag_page(frag),
 728                                  skb_frag_address(frag), cur_len, frag_len);
 729                 len -= cur_len;
 730                 frag++;
 731         }
 732 }
 733
 734 static struct sk_buff *
 735 __ieee80211_amsdu_copy(struct sk_buff *skb, unsigned int hlen,
 736                        int offset, int len, bool reuse_frag,
 737                        int min_len)
 738 {
 739         struct sk_buff *frame;
 740         int cur_len = len;
 741
 742         if (skb->len - offset < len)
 743                 return NULL;
 744
 745         /*
 746          * When reusing framents, copy some data to the head to simplify
 747          * ethernet header handling and speed up protocol header processing
 748          * in the stack later.
 749          */
 750         if (reuse_frag)
 751                 cur_len = min_t(int, len, min_len);
 752
 753         /*
 754          * Allocate and reserve two bytes more for payload
 755          * alignment since sizeof(struct ethhdr) is 14.
 756          */
 757         frame = dev_alloc_skb(hlen + sizeof(struct ethhdr) + 2 + cur_len);
 758         if (!frame)
 759                 return NULL;
 760
 761         frame->priority = skb->priority;
 762         skb_reserve(frame, hlen + sizeof(struct ethhdr) + 2);
 763         skb_copy_bits(skb, offset, skb_put(frame, cur_len), cur_len);
 764
 765         len -= cur_len;
 766         if (!len)
 767                 return frame;
 768
 769         offset += cur_len;
 770         __ieee80211_amsdu_copy_frag(skb, frame, offset, len);
 771
 772         return frame;
 773 }
 774
 775 static u16
 776 ieee80211_amsdu_subframe_length(void *field, u8 mesh_flags, u8 hdr_type)
 777 {
 778         __le16 *field_le = field;
 779         __be16 *field_be = field;
 780         u16 len;
 781
 782         if (hdr_type >= 2)
 783                 len = le16_to_cpu(*field_le);
 784         else
 785                 len = be16_to_cpu(*field_be);
 786         if (hdr_type)
 787                 len += __ieee80211_get_mesh_hdrlen(mesh_flags);
 788
 789         return len;
 790 }
 791
 792 bool ieee80211_is_valid_amsdu(struct sk_buff *skb, u8 mesh_hdr)
 793 {
 794         int offset = 0, remaining, subframe_len, padding;
 795
 796         for (offset = 0; offset < skb->len; offset += subframe_len + padding) {
 797                 struct {
 798                     __be16 len;
 799                     u8 mesh_flags;
 800                 } hdr;
 801                 u16 len;
 802
 803                 if (skb_copy_bits(skb, offset + 2 * ETH_ALEN, &hdr, sizeof(hdr)) < 0)
 804                         return false;
 805
 806                 len = ieee80211_amsdu_subframe_length(&hdr.len, hdr.mesh_flags,
 807                                                       mesh_hdr);
 808                 subframe_len = sizeof(struct ethhdr) + len;
 809                 padding = (4 - subframe_len) & 0x3;
 810                 remaining = skb->len - offset;
 811
 812                 if (subframe_len > remaining)
 813                         return false;
 814         }
 815
 816         return true;
 817 }
 818 EXPORT_SYMBOL(ieee80211_is_valid_amsdu);
 819
 820 void ieee80211_amsdu_to_8023s(struct sk_buff *skb, struct sk_buff_head *list,
 821                               const u8 *addr, enum nl80211_iftype iftype,
 822                               const unsigned int extra_headroom,
 823                               const u8 *check_da, const u8 *check_sa,
 824                               u8 mesh_control)
 825 {
 826         unsigned int hlen = ALIGN(extra_headroom, 4);
 827         struct sk_buff *frame = NULL;
 828         int offset = 0, remaining;
 829         struct {
 830                 struct ethhdr eth;
 831                 uint8_t flags;
 832         } hdr;
 833         bool reuse_frag = skb->head_frag && !skb_has_frag_list(skb);
 834         bool reuse_skb = false;
 835         bool last = false;
 836         int copy_len = sizeof(hdr.eth);
 837
 838         if (iftype == NL80211_IFTYPE_MESH_POINT)
 839                 copy_len = sizeof(hdr);
 840
 841         while (!last) {
 842                 unsigned int subframe_len;
 843                 int len, mesh_len = 0;
 844                 u8 padding;
 845
 846                 skb_copy_bits(skb, offset, &hdr, copy_len);
 847                 if (iftype == NL80211_IFTYPE_MESH_POINT)
 848                         mesh_len = __ieee80211_get_mesh_hdrlen(hdr.flags);
 849                 len = ieee80211_amsdu_subframe_length(&hdr.eth.h_proto, hdr.flags,
 850                                                       mesh_control);
 851                 subframe_len = sizeof(struct ethhdr) + len;
 852                 padding = (4 - subframe_len) & 0x3;
 853
 854                 /* the last MSDU has no padding */
 855                 remaining = skb->len - offset;
 856                 if (subframe_len > remaining)
 857                         goto purge;
 858                 /* mitigate A-MSDU aggregation injection attacks */
 859                 if (ether_addr_equal(hdr.eth.h_dest, rfc1042_header))
 860                         goto purge;
 861
 862                 offset += sizeof(struct ethhdr);
 863                 last = remaining <= subframe_len + padding;
 864
 865                 /* FIXME: should we really accept multicast DA? */
 866                 if ((check_da && !is_multicast_ether_addr(hdr.eth.h_dest) &&
 867                      !ether_addr_equal(check_da, hdr.eth.h_dest)) ||
 868                     (check_sa && !ether_addr_equal(check_sa, hdr.eth.h_source))) {
 869                         offset += len + padding;
 870                         continue;
 871                 }
 872
 873                 /* reuse skb for the last subframe */
 874                 if (!skb_is_nonlinear(skb) && !reuse_frag && last) {
 875                         skb_pull(skb, offset);
 876                         frame = skb;
 877                         reuse_skb = true;
 878                 } else {
 879                         frame = __ieee80211_amsdu_copy(skb, hlen, offset, len,
 880                                                        reuse_frag, 32 + mesh_len);
 881                         if (!frame)
 882                                 goto purge;
 883
 884                         offset += len + padding;
 885                 }
 886
 887                 skb_reset_network_header(frame);
 888                 frame->dev = skb->dev;
 889                 frame->priority = skb->priority;
 890
 891                 if (likely(iftype != NL80211_IFTYPE_MESH_POINT &&
 892                            ieee80211_get_8023_tunnel_proto(frame->data, &hdr.eth.h_proto)))
 893                         skb_pull(frame, ETH_ALEN + 2);
 894
 895                 memcpy(skb_push(frame, sizeof(hdr.eth)), &hdr.eth, sizeof(hdr.eth));
 896                 __skb_queue_tail(list, frame);
 897         }
 898
 899         if (!reuse_skb)
 900                 dev_kfree_skb(skb);
 901
 902         return;
 903
 904  purge:
 905         __skb_queue_purge(list);
 906         dev_kfree_skb(skb);
 907 }
 908 EXPORT_SYMBOL(ieee80211_amsdu_to_8023s);
 909
 910 /* Given a data frame determine the 802.1p/1d tag to use. */
 911 unsigned int cfg80211_classify8021d(struct sk_buff *skb,
 912                                     struct cfg80211_qos_map *qos_map)
 913 {
 914         unsigned int dscp;
 915         unsigned char vlan_priority;
 916         unsigned int ret;
 917
 918         /* skb->priority values from 256->263 are magic values to
 919          * directly indicate a specific 802.1d priority.  This is used
 920          * to allow 802.1d priority to be passed directly in from VLAN
 921          * tags, etc.
 922          */
 923         if (skb->priority >= 256 && skb->priority <= 263) {
 924                 ret = skb->priority - 256;
 925                 goto out;
 926         }
 927
 928         if (skb_vlan_tag_present(skb)) {
 929                 vlan_priority = (skb_vlan_tag_get(skb) & VLAN_PRIO_MASK)
 930                         >> VLAN_PRIO_SHIFT;
 931                 if (vlan_priority > 0) {
 932                         ret = vlan_priority;
 933                         goto out;
 934                 }
 935         }
 936
 937         switch (skb->protocol) {
 938         case htons(ETH_P_IP):
 939                 dscp = ipv4_get_dsfield(ip_hdr(skb)) & 0xfc;
 940                 break;
 941         case htons(ETH_P_IPV6):
 942                 dscp = ipv6_get_dsfield(ipv6_hdr(skb)) & 0xfc;
 943                 break;
 944         case htons(ETH_P_MPLS_UC):
 945         case htons(ETH_P_MPLS_MC): {
 946                 struct mpls_label mpls_tmp, *mpls;
 947
 948                 mpls = skb_header_pointer(skb, sizeof(struct ethhdr),
 949                                           sizeof(*mpls), &mpls_tmp);
 950                 if (!mpls)
 951                         return 0;
 952
 953                 ret = (ntohl(mpls->entry) & MPLS_LS_TC_MASK)
 954                         >> MPLS_LS_TC_SHIFT;
 955                 goto out;
 956         }
 957         case htons(ETH_P_80221):
 958                 /* 802.21 is always network control traffic */
 959                 return 7;
 960         default:
 961                 return 0;
 962         }
 963
 964         if (qos_map) {
 965                 unsigned int i, tmp_dscp = dscp >> 2;
 966
 967                 for (i = 0; i < qos_map->num_des; i++) {
 968                         if (tmp_dscp == qos_map->dscp_exception[i].dscp) {
 969                                 ret = qos_map->dscp_exception[i].up;
 970                                 goto out;
 971                         }
 972                 }
 973
 974                 for (i = 0; i < 8; i++) {
 975                         if (tmp_dscp >= qos_map->up[i].low &&
 976                             tmp_dscp <= qos_map->up[i].high) {
 977                                 ret = i;
 978                                 goto out;
 979                         }
 980                 }
 981         }
 982
 983         ret = dscp >> 5;
 984 out:
 985         return array_index_nospec(ret, IEEE80211_NUM_TIDS);
 986 }
 987 EXPORT_SYMBOL(cfg80211_classify8021d);
 988
 989 const struct element *ieee80211_bss_get_elem(struct cfg80211_bss *bss, u8 id)
 990 {
 991         const struct cfg80211_bss_ies *ies;
 992
 993         ies = rcu_dereference(bss->ies);
 994         if (!ies)
 995                 return NULL;
 996
 997         return cfg80211_find_elem(id, ies->data, ies->len);
 998 }
 999 EXPORT_SYMBOL(ieee80211_bss_get_elem);
1000
1001 void cfg80211_upload_connect_keys(struct wireless_dev *wdev)
1002 {
1003         struct cfg80211_registered_device *rdev = wiphy_to_rdev(wdev->wiphy);
1004         struct net_device *dev = wdev->netdev;
1005         int i;
1006
1007         if (!wdev->connect_keys)
1008                 return;
1009
1010         for (i = 0; i < 4; i++) {
1011                 if (!wdev->connect_keys->params[i].cipher)
1012                         continue;
1013                 if (rdev_add_key(rdev, dev, -1, i, false, NULL,
1014                                  &wdev->connect_keys->params[i])) {
1015                         netdev_err(dev, "failed to set key %d\n", i);
1016                         continue;
1017                 }
1018                 if (wdev->connect_keys->def == i &&
1019                     rdev_set_default_key(rdev, dev, -1, i, true, true)) {
1020                         netdev_err(dev, "failed to set defkey %d\n", i);
1021                         continue;
1022                 }
1023         }
1024
1025         kfree_sensitive(wdev->connect_keys);
1026         wdev->connect_keys = NULL;
1027 }
1028
1029 void cfg80211_process_wdev_events(struct wireless_dev *wdev)
1030 {
1031         struct cfg80211_event *ev;
1032         unsigned long flags;
1033
1034         spin_lock_irqsave(&wdev->event_lock, flags);
1035         while (!list_empty(&wdev->event_list)) {
1036                 ev = list_first_entry(&wdev->event_list,
1037                                       struct cfg80211_event, list);
1038                 list_del(&ev->list);
1039                 spin_unlock_irqrestore(&wdev->event_lock, flags);
1040
1041                 switch (ev->type) {
1042                 case EVENT_CONNECT_RESULT:
1043                         __cfg80211_connect_result(
1044                                 wdev->netdev,
1045                                 &ev->cr,
1046                                 ev->cr.status == WLAN_STATUS_SUCCESS);
1047                         break;
1048                 case EVENT_ROAMED:
1049                         __cfg80211_roamed(wdev, &ev->rm);
1050                         break;
1051                 case EVENT_DISCONNECTED:
1052                         __cfg80211_disconnected(wdev->netdev,
1053                                                 ev->dc.ie, ev->dc.ie_len,
1054                                                 ev->dc.reason,
1055                                                 !ev->dc.locally_generated);
1056                         break;
1057                 case EVENT_IBSS_JOINED:
1058                         __cfg80211_ibss_joined(wdev->netdev, ev->ij.bssid,
1059                                                ev->ij.channel);
1060                         break;
1061                 case EVENT_STOPPED:
1062                         cfg80211_leave(wiphy_to_rdev(wdev->wiphy), wdev);
1063                         break;
1064                 case EVENT_PORT_AUTHORIZED:
1065                         __cfg80211_port_authorized(wdev, ev->pa.peer_addr,
1066                                                    ev->pa.td_bitmap,
1067                                                    ev->pa.td_bitmap_len);
1068                         break;
1069                 }
1070
1071                 kfree(ev);
1072
1073                 spin_lock_irqsave(&wdev->event_lock, flags);
1074         }
1075         spin_unlock_irqrestore(&wdev->event_lock, flags);
1076 }
1077
1078 void cfg80211_process_rdev_events(struct cfg80211_registered_device *rdev)
1079 {
1080         struct wireless_dev *wdev;
1081
1082         lockdep_assert_held(&rdev->wiphy.mtx);
1083
1084         list_for_each_entry(wdev, &rdev->wiphy.wdev_list, list)
1085                 cfg80211_process_wdev_events(wdev);
1086 }
1087
1088 int cfg80211_change_iface(struct cfg80211_registered_device *rdev,
1089                           struct net_device *dev, enum nl80211_iftype ntype,
1090                           struct vif_params *params)
1091 {
1092         int err;
1093         enum nl80211_iftype otype = dev->ieee80211_ptr->iftype;
1094
1095         lockdep_assert_held(&rdev->wiphy.mtx);
1096
1097         /* don't support changing VLANs, you just re-create them */
1098         if (otype == NL80211_IFTYPE_AP_VLAN)
1099                 return -EOPNOTSUPP;
1100
1101         /* cannot change into P2P device or NAN */
1102         if (ntype == NL80211_IFTYPE_P2P_DEVICE ||
1103             ntype == NL80211_IFTYPE_NAN)
1104                 return -EOPNOTSUPP;
1105
1106         if (!rdev->ops->change_virtual_intf ||
1107             !(rdev->wiphy.interface_modes & (1 << ntype)))
1108                 return -EOPNOTSUPP;
1109
1110         if (ntype != otype) {
1111                 /* if it's part of a bridge, reject changing type to station/ibss */
1112                 if (netif_is_bridge_port(dev) &&
1113                     (ntype == NL80211_IFTYPE_ADHOC ||
1114                      ntype == NL80211_IFTYPE_STATION ||
1115                      ntype == NL80211_IFTYPE_P2P_CLIENT))
1116                         return -EBUSY;
1117
1118                 dev->ieee80211_ptr->use_4addr = false;
1119                 rdev_set_qos_map(rdev, dev, NULL);
1120
1121                 switch (otype) {
1122                 case NL80211_IFTYPE_AP:
1123                 case NL80211_IFTYPE_P2P_GO:
1124                         cfg80211_stop_ap(rdev, dev, -1, true);
1125                         break;
1126                 case NL80211_IFTYPE_ADHOC:
1127                         cfg80211_leave_ibss(rdev, dev, false);
1128                         break;
1129                 case NL80211_IFTYPE_STATION:
1130                 case NL80211_IFTYPE_P2P_CLIENT:
1131                         cfg80211_disconnect(rdev, dev,
1132                                             WLAN_REASON_DEAUTH_LEAVING, true);
1133                         break;
1134                 case NL80211_IFTYPE_MESH_POINT:
1135                         /* mesh should be handled? */
1136                         break;
1137                 case NL80211_IFTYPE_OCB:
1138                         cfg80211_leave_ocb(rdev, dev);
1139                         break;
1140                 default:
1141                         break;
1142                 }
1143
1144                 cfg80211_process_rdev_events(rdev);
1145                 cfg80211_mlme_purge_registrations(dev->ieee80211_ptr);
1146
1147                 memset(&dev->ieee80211_ptr->u, 0,
1148                        sizeof(dev->ieee80211_ptr->u));
1149                 memset(&dev->ieee80211_ptr->links, 0,
1150                        sizeof(dev->ieee80211_ptr->links));
1151         }
1152
1153         err = rdev_change_virtual_intf(rdev, dev, ntype, params);
1154
1155         WARN_ON(!err && dev->ieee80211_ptr->iftype != ntype);
1156
1157         if (!err && params && params->use_4addr != -1)
1158                 dev->ieee80211_ptr->use_4addr = params->use_4addr;
1159
1160         if (!err) {
1161                 dev->priv_flags &= ~IFF_DONT_BRIDGE;
1162                 switch (ntype) {
1163                 case NL80211_IFTYPE_STATION:
1164                         if (dev->ieee80211_ptr->use_4addr)
1165                                 break;
1166                         fallthrough;
1167                 case NL80211_IFTYPE_OCB:
1168                 case NL80211_IFTYPE_P2P_CLIENT:
1169                 case NL80211_IFTYPE_ADHOC:
1170                         dev->priv_flags |= IFF_DONT_BRIDGE;
1171                         break;
1172                 case NL80211_IFTYPE_P2P_GO:
1173                 case NL80211_IFTYPE_AP:
1174                 case NL80211_IFTYPE_AP_VLAN:
1175                 case NL80211_IFTYPE_MESH_POINT:
1176                         /* bridging OK */
1177                         break;
1178                 case NL80211_IFTYPE_MONITOR:
1179                         /* monitor can't bridge anyway */
1180                         break;
1181                 case NL80211_IFTYPE_UNSPECIFIED:
1182                 case NUM_NL80211_IFTYPES:
1183                         /* not happening */
1184                         break;
1185                 case NL80211_IFTYPE_P2P_DEVICE:
1186                 case NL80211_IFTYPE_WDS:
1187                 case NL80211_IFTYPE_NAN:
1188                         WARN_ON(1);
1189                         break;
1190                 }
1191         }
1192
1193         if (!err && ntype != otype && netif_running(dev)) {
1194                 cfg80211_update_iface_num(rdev, ntype, 1);
1195                 cfg80211_update_iface_num(rdev, otype, -1);
1196         }
1197
1198         return err;
1199 }
1200
1201 static u32 cfg80211_calculate_bitrate_ht(struct rate_info *rate)
1202 {
1203         int modulation, streams, bitrate;
1204
1205         /* the formula below does only work for MCS values smaller than 32 */
1206         if (WARN_ON_ONCE(rate->mcs >= 32))
1207                 return 0;
1208
1209         modulation = rate->mcs & 7;
1210         streams = (rate->mcs >> 3) + 1;
1211
1212         bitrate = (rate->bw == RATE_INFO_BW_40) ? 13500000 : 6500000;
1213
1214         if (modulation < 4)
1215                 bitrate *= (modulation + 1);
1216         else if (modulation == 4)
1217                 bitrate *= (modulation + 2);
1218         else
1219                 bitrate *= (modulation + 3);
1220
1221         bitrate *= streams;
1222
1223         if (rate->flags & RATE_INFO_FLAGS_SHORT_GI)
1224                 bitrate = (bitrate / 9) * 10;
1225
1226         /* do NOT round down here */
1227         return (bitrate + 50000) / 100000;
1228 }
1229
1230 static u32 cfg80211_calculate_bitrate_dmg(struct rate_info *rate)
1231 {
1232         static const u32 __mcs2bitrate[] = {
1233                 /* control PHY */
1234                 [0] =   275,
1235                 /* SC PHY */
1236                 [1] =  3850,
1237                 [2] =  7700,
1238                 [3] =  9625,
1239                 [4] = 11550,
1240                 [5] = 12512, /* 1251.25 mbps */
1241                 [6] = 15400,
1242                 [7] = 19250,
1243                 [8] = 23100,
1244                 [9] = 25025,
1245                 [10] = 30800,
1246                 [11] = 38500,
1247                 [12] = 46200,
1248                 /* OFDM PHY */
1249                 [13] =  6930,
1250                 [14] =  8662, /* 866.25 mbps */
1251                 [15] = 13860,
1252                 [16] = 17325,
1253                 [17] = 20790,
1254                 [18] = 27720,
1255                 [19] = 34650,
1256                 [20] = 41580,
1257                 [21] = 45045,
1258                 [22] = 51975,
1259                 [23] = 62370,
1260                 [24] = 67568, /* 6756.75 mbps */
1261                 /* LP-SC PHY */
1262                 [25] =  6260,
1263                 [26] =  8340,
1264                 [27] = 11120,
1265                 [28] = 12510,
1266                 [29] = 16680,
1267                 [30] = 22240,
1268                 [31] = 25030,
1269         };
1270
1271         if (WARN_ON_ONCE(rate->mcs >= ARRAY_SIZE(__mcs2bitrate)))
1272                 return 0;
1273
1274         return __mcs2bitrate[rate->mcs];
1275 }
1276
1277 static u32 cfg80211_calculate_bitrate_extended_sc_dmg(struct rate_info *rate)
1278 {
1279         static const u32 __mcs2bitrate[] = {
1280                 [6 - 6] = 26950, /* MCS 9.1 : 2695.0 mbps */
1281                 [7 - 6] = 50050, /* MCS 12.1 */
1282                 [8 - 6] = 53900,
1283                 [9 - 6] = 57750,
1284                 [10 - 6] = 63900,
1285                 [11 - 6] = 75075,
1286                 [12 - 6] = 80850,
1287         };
1288
1289         /* Extended SC MCS not defined for base MCS below 6 or above 12 */
1290         if (WARN_ON_ONCE(rate->mcs < 6 || rate->mcs > 12))
1291                 return 0;
1292
1293         return __mcs2bitrate[rate->mcs - 6];
1294 }
1295
1296 static u32 cfg80211_calculate_bitrate_edmg(struct rate_info *rate)
1297 {
1298         static const u32 __mcs2bitrate[] = {
1299                 /* control PHY */
1300                 [0] =   275,
1301                 /* SC PHY */
1302                 [1] =  3850,
1303                 [2] =  7700,
1304                 [3] =  9625,
1305                 [4] = 11550,
1306                 [5] = 12512, /* 1251.25 mbps */
1307                 [6] = 13475,
1308                 [7] = 15400,
1309                 [8] = 19250,
1310                 [9] = 23100,
1311                 [10] = 25025,
1312                 [11] = 26950,
1313                 [12] = 30800,
1314                 [13] = 38500,
1315                 [14] = 46200,
1316                 [15] = 50050,
1317                 [16] = 53900,
1318                 [17] = 57750,
1319                 [18] = 69300,
1320                 [19] = 75075,
1321                 [20] = 80850,
1322         };
1323
1324         if (WARN_ON_ONCE(rate->mcs >= ARRAY_SIZE(__mcs2bitrate)))
1325                 return 0;
1326
1327         return __mcs2bitrate[rate->mcs] * rate->n_bonded_ch;
1328 }
1329
1330 static u32 cfg80211_calculate_bitrate_vht(struct rate_info *rate)
1331 {
1332         static const u32 base[4][12] = {
1333                 {   6500000,
1334                    13000000,
1335                    19500000,
1336                    26000000,
1337                    39000000,
1338                    52000000,
1339                    58500000,
1340                    65000000,
1341                    78000000,
1342                 /* not in the spec, but some devices use this: */
1343                    86700000,
1344                    97500000,
1345                   108300000,
1346                 },
1347                 {  13500000,
1348                    27000000,
1349                    40500000,
1350                    54000000,
1351                    81000000,
1352                   108000000,
1353                   121500000,
1354                   135000000,
1355                   162000000,
1356                   180000000,
1357                   202500000,
1358                   225000000,
1359                 },
1360                 {  29300000,
1361                    58500000,
1362                    87800000,
1363                   117000000,
1364                   175500000,
1365                   234000000,
1366                   263300000,
1367                   292500000,
1368                   351000000,
1369                   390000000,
1370                   438800000,
1371                   487500000,
1372                 },
1373                 {  58500000,
1374                   117000000,
1375                   175500000,
1376                   234000000,
1377                   351000000,
1378                   468000000,
1379                   526500000,
1380                   585000000,
1381                   702000000,
1382                   780000000,
1383                   877500000,
1384                   975000000,
1385                 },
1386         };
1387         u32 bitrate;
1388         int idx;
1389
1390         if (rate->mcs > 11)
1391                 goto warn;
1392
1393         switch (rate->bw) {
1394         case RATE_INFO_BW_160:
1395                 idx = 3;
1396                 break;
1397         case RATE_INFO_BW_80:
1398                 idx = 2;
1399                 break;
1400         case RATE_INFO_BW_40:
1401                 idx = 1;
1402                 break;
1403         case RATE_INFO_BW_5:
1404         case RATE_INFO_BW_10:
1405         default:
1406                 goto warn;
1407         case RATE_INFO_BW_20:
1408                 idx = 0;
1409         }
1410
1411         bitrate = base[idx][rate->mcs];
1412         bitrate *= rate->nss;
1413
1414         if (rate->flags & RATE_INFO_FLAGS_SHORT_GI)
1415                 bitrate = (bitrate / 9) * 10;
1416
1417         /* do NOT round down here */
1418         return (bitrate + 50000) / 100000;
1419  warn:
1420         WARN_ONCE(1, "invalid rate bw=%d, mcs=%d, nss=%d\n",
1421                   rate->bw, rate->mcs, rate->nss);
1422         return 0;
1423 }
1424
1425 static u32 cfg80211_calculate_bitrate_he(struct rate_info *rate)
1426 {
1427 #define SCALE 6144
1428         u32 mcs_divisors[14] = {
1429                 102399, /* 16.666666... */
1430                  51201, /*  8.333333... */
1431                  34134, /*  5.555555... */
1432                  25599, /*  4.166666... */
1433                  17067, /*  2.777777... */
1434                  12801, /*  2.083333... */
1435                  11377, /*  1.851725... */
1436                  10239, /*  1.666666... */
1437                   8532, /*  1.388888... */
1438                   7680, /*  1.250000... */
1439                   6828, /*  1.111111... */
1440                   6144, /*  1.000000... */
1441                   5690, /*  0.926106... */
1442                   5120, /*  0.833333... */
1443         };
1444         u32 rates_160M[3] = { 960777777, 907400000, 816666666 };
1445         u32 rates_969[3] =  { 480388888, 453700000, 408333333 };
1446         u32 rates_484[3] =  { 229411111, 216666666, 195000000 };
1447         u32 rates_242[3] =  { 114711111, 108333333,  97500000 };
1448         u32 rates_106[3] =  {  40000000,  37777777,  34000000 };
1449         u32 rates_52[3]  =  {  18820000,  17777777,  16000000 };
1450         u32 rates_26[3]  =  {   9411111,   8888888,   8000000 };
1451         u64 tmp;
1452         u32 result;
1453
1454         if (WARN_ON_ONCE(rate->mcs > 13))
1455                 return 0;
1456
1457         if (WARN_ON_ONCE(rate->he_gi > NL80211_RATE_INFO_HE_GI_3_2))
1458                 return 0;
1459         if (WARN_ON_ONCE(rate->he_ru_alloc >
1460                          NL80211_RATE_INFO_HE_RU_ALLOC_2x996))
1461                 return 0;
1462         if (WARN_ON_ONCE(rate->nss < 1 || rate->nss > 8))
1463                 return 0;
1464
1465         if (rate->bw == RATE_INFO_BW_160)
1466                 result = rates_160M[rate->he_gi];
1467         else if (rate->bw == RATE_INFO_BW_80 ||
1468                  (rate->bw == RATE_INFO_BW_HE_RU &&
1469                   rate->he_ru_alloc == NL80211_RATE_INFO_HE_RU_ALLOC_996))
1470                 result = rates_969[rate->he_gi];
1471         else if (rate->bw == RATE_INFO_BW_40 ||
1472                  (rate->bw == RATE_INFO_BW_HE_RU &&
1473                   rate->he_ru_alloc == NL80211_RATE_INFO_HE_RU_ALLOC_484))
1474                 result = rates_484[rate->he_gi];
1475         else if (rate->bw == RATE_INFO_BW_20 ||
1476                  (rate->bw == RATE_INFO_BW_HE_RU &&
1477                   rate->he_ru_alloc == NL80211_RATE_INFO_HE_RU_ALLOC_242))
1478                 result = rates_242[rate->he_gi];
1479         else if (rate->bw == RATE_INFO_BW_HE_RU &&
1480                  rate->he_ru_alloc == NL80211_RATE_INFO_HE_RU_ALLOC_106)
1481                 result = rates_106[rate->he_gi];
1482         else if (rate->bw == RATE_INFO_BW_HE_RU &&
1483                  rate->he_ru_alloc == NL80211_RATE_INFO_HE_RU_ALLOC_52)
1484                 result = rates_52[rate->he_gi];
1485         else if (rate->bw == RATE_INFO_BW_HE_RU &&
1486                  rate->he_ru_alloc == NL80211_RATE_INFO_HE_RU_ALLOC_26)
1487                 result = rates_26[rate->he_gi];
1488         else {
1489                 WARN(1, "invalid HE MCS: bw:%d, ru:%d\n",
1490                      rate->bw, rate->he_ru_alloc);
1491                 return 0;
1492         }
1493
1494         /* now scale to the appropriate MCS */
1495         tmp = result;
1496         tmp *= SCALE;
1497         do_div(tmp, mcs_divisors[rate->mcs]);
1498         result = tmp;
1499
1500         /* and take NSS, DCM into account */
1501         result = (result * rate->nss) / 8;
1502         if (rate->he_dcm)
1503                 result /= 2;
1504
1505         return result / 10000;
1506 }
1507
1508 static u32 cfg80211_calculate_bitrate_eht(struct rate_info *rate)
1509 {
1510 #define SCALE 6144
1511         static const u32 mcs_divisors[16] = {
1512                 102399, /* 16.666666... */
1513                  51201, /*  8.333333... */
1514                  34134, /*  5.555555... */
1515                  25599, /*  4.166666... */
1516                  17067, /*  2.777777... */
1517                  12801, /*  2.083333... */
1518                  11377, /*  1.851725... */
1519                  10239, /*  1.666666... */
1520                   8532, /*  1.388888... */
1521                   7680, /*  1.250000... */
1522                   6828, /*  1.111111... */
1523                   6144, /*  1.000000... */
1524                   5690, /*  0.926106... */
1525                   5120, /*  0.833333... */
1526                 409600, /* 66.666666... */
1527                 204800, /* 33.333333... */
1528         };
1529         static const u32 rates_996[3] =  { 480388888, 453700000, 408333333 };
1530         static const u32 rates_484[3] =  { 229411111, 216666666, 195000000 };
1531         static const u32 rates_242[3] =  { 114711111, 108333333,  97500000 };
1532         static const u32 rates_106[3] =  {  40000000,  37777777,  34000000 };
1533         static const u32 rates_52[3]  =  {  18820000,  17777777,  16000000 };
1534         static const u32 rates_26[3]  =  {   9411111,   8888888,   8000000 };
1535         u64 tmp;
1536         u32 result;
1537
1538         if (WARN_ON_ONCE(rate->mcs > 15))
1539                 return 0;
1540         if (WARN_ON_ONCE(rate->eht_gi > NL80211_RATE_INFO_EHT_GI_3_2))
1541                 return 0;
1542         if (WARN_ON_ONCE(rate->eht_ru_alloc >
1543                          NL80211_RATE_INFO_EHT_RU_ALLOC_4x996))
1544                 return 0;
1545         if (WARN_ON_ONCE(rate->nss < 1 || rate->nss > 8))
1546                 return 0;
1547
1548         /* Bandwidth checks for MCS 14 */
1549         if (rate->mcs == 14) {
1550                 if ((rate->bw != RATE_INFO_BW_EHT_RU &&
1551                      rate->bw != RATE_INFO_BW_80 &&
1552                      rate->bw != RATE_INFO_BW_160 &&
1553                      rate->bw != RATE_INFO_BW_320) ||
1554                     (rate->bw == RATE_INFO_BW_EHT_RU &&
1555                      rate->eht_ru_alloc != NL80211_RATE_INFO_EHT_RU_ALLOC_996 &&
1556                      rate->eht_ru_alloc != NL80211_RATE_INFO_EHT_RU_ALLOC_2x996 &&
1557                      rate->eht_ru_alloc != NL80211_RATE_INFO_EHT_RU_ALLOC_4x996)) {
1558                         WARN(1, "invalid EHT BW for MCS 14: bw:%d, ru:%d\n",
1559                              rate->bw, rate->eht_ru_alloc);
1560                         return 0;
1561                 }
1562         }
1563
1564         if (rate->bw == RATE_INFO_BW_320 ||
1565             (rate->bw == RATE_INFO_BW_EHT_RU &&
1566              rate->eht_ru_alloc == NL80211_RATE_INFO_EHT_RU_ALLOC_4x996))
1567                 result = 4 * rates_996[rate->eht_gi];
1568         else if (rate->bw == RATE_INFO_BW_EHT_RU &&
1569                  rate->eht_ru_alloc == NL80211_RATE_INFO_EHT_RU_ALLOC_3x996P484)
1570                 result = 3 * rates_996[rate->eht_gi] + rates_484[rate->eht_gi];
1571         else if (rate->bw == RATE_INFO_BW_EHT_RU &&
1572                  rate->eht_ru_alloc == NL80211_RATE_INFO_EHT_RU_ALLOC_3x996)
1573                 result = 3 * rates_996[rate->eht_gi];
1574         else if (rate->bw == RATE_INFO_BW_EHT_RU &&
1575                  rate->eht_ru_alloc == NL80211_RATE_INFO_EHT_RU_ALLOC_2x996P484)
1576                 result = 2 * rates_996[rate->eht_gi] + rates_484[rate->eht_gi];
1577         else if (rate->bw == RATE_INFO_BW_160 ||
1578                  (rate->bw == RATE_INFO_BW_EHT_RU &&
1579                   rate->eht_ru_alloc == NL80211_RATE_INFO_EHT_RU_ALLOC_2x996))
1580                 result = 2 * rates_996[rate->eht_gi];
1581         else if (rate->bw == RATE_INFO_BW_EHT_RU &&
1582                  rate->eht_ru_alloc ==
1583                  NL80211_RATE_INFO_EHT_RU_ALLOC_996P484P242)
1584                 result = rates_996[rate->eht_gi] + rates_484[rate->eht_gi]
1585                          + rates_242[rate->eht_gi];
1586         else if (rate->bw == RATE_INFO_BW_EHT_RU &&
1587                  rate->eht_ru_alloc == NL80211_RATE_INFO_EHT_RU_ALLOC_996P484)
1588                 result = rates_996[rate->eht_gi] + rates_484[rate->eht_gi];
1589         else if (rate->bw == RATE_INFO_BW_80 ||
1590                  (rate->bw == RATE_INFO_BW_EHT_RU &&
1591                   rate->eht_ru_alloc == NL80211_RATE_INFO_EHT_RU_ALLOC_996))
1592                 result = rates_996[rate->eht_gi];
1593         else if (rate->bw == RATE_INFO_BW_EHT_RU &&
1594                  rate->eht_ru_alloc == NL80211_RATE_INFO_EHT_RU_ALLOC_484P242)
1595                 result = rates_484[rate->eht_gi] + rates_242[rate->eht_gi];
1596         else if (rate->bw == RATE_INFO_BW_40 ||
1597                  (rate->bw == RATE_INFO_BW_EHT_RU &&
1598                   rate->eht_ru_alloc == NL80211_RATE_INFO_EHT_RU_ALLOC_484))
1599                 result = rates_484[rate->eht_gi];
1600         else if (rate->bw == RATE_INFO_BW_20 ||
1601                  (rate->bw == RATE_INFO_BW_EHT_RU &&
1602                   rate->eht_ru_alloc == NL80211_RATE_INFO_EHT_RU_ALLOC_242))
1603                 result = rates_242[rate->eht_gi];
1604         else if (rate->bw == RATE_INFO_BW_EHT_RU &&
1605                  rate->eht_ru_alloc == NL80211_RATE_INFO_EHT_RU_ALLOC_106P26)
1606                 result = rates_106[rate->eht_gi] + rates_26[rate->eht_gi];
1607         else if (rate->bw == RATE_INFO_BW_EHT_RU &&
1608                  rate->eht_ru_alloc == NL80211_RATE_INFO_EHT_RU_ALLOC_106)
1609                 result = rates_106[rate->eht_gi];
1610         else if (rate->bw == RATE_INFO_BW_EHT_RU &&
1611                  rate->eht_ru_alloc == NL80211_RATE_INFO_EHT_RU_ALLOC_52P26)
1612                 result = rates_52[rate->eht_gi] + rates_26[rate->eht_gi];
1613         else if (rate->bw == RATE_INFO_BW_EHT_RU &&
1614                  rate->eht_ru_alloc == NL80211_RATE_INFO_EHT_RU_ALLOC_52)
1615                 result = rates_52[rate->eht_gi];
1616         else if (rate->bw == RATE_INFO_BW_EHT_RU &&
1617                  rate->eht_ru_alloc == NL80211_RATE_INFO_EHT_RU_ALLOC_26)
1618                 result = rates_26[rate->eht_gi];
1619         else {
1620                 WARN(1, "invalid EHT MCS: bw:%d, ru:%d\n",
1621                      rate->bw, rate->eht_ru_alloc);
1622                 return 0;
1623         }
1624
1625         /* now scale to the appropriate MCS */
1626         tmp = result;
1627         tmp *= SCALE;
1628         do_div(tmp, mcs_divisors[rate->mcs]);
1629
1630         /* and take NSS */
1631         tmp *= rate->nss;
1632         do_div(tmp, 8);
1633
1634         result = tmp;
1635
1636         return result / 10000;
1637 }
1638
1639 static u32 cfg80211_calculate_bitrate_s1g(struct rate_info *rate)
1640 {
1641         /* For 1, 2, 4, 8 and 16 MHz channels */
1642         static const u32 base[5][11] = {
1643                 {  300000,
1644                    600000,
1645                    900000,
1646                   1200000,
1647                   1800000,
1648                   2400000,
1649                   2700000,
1650                   3000000,
1651                   3600000,
1652                   4000000,
1653                   /* MCS 10 supported in 1 MHz only */
1654                   150000,
1655                 },
1656                 {  650000,
1657                   1300000,
1658                   1950000,
1659                   2600000,
1660                   3900000,
1661                   5200000,
1662                   5850000,
1663                   6500000,
1664                   7800000,
1665                   /* MCS 9 not valid */
1666                 },
1667                 {  1350000,
1668                    2700000,
1669                    4050000,
1670                    5400000,
1671                    8100000,
1672                   10800000,
1673                   12150000,
1674                   13500000,
1675                   16200000,
1676                   18000000,
1677                 },
1678                 {  2925000,
1679                    5850000,
1680                    8775000,
1681                   11700000,
1682                   17550000,
1683                   23400000,
1684                   26325000,
1685                   29250000,
1686                   35100000,
1687                   39000000,
1688                 },
1689                 {  8580000,
1690                   11700000,
1691                   17550000,
1692                   23400000,
1693                   35100000,
1694                   46800000,
1695                   52650000,
1696                   58500000,
1697                   70200000,
1698                   78000000,
1699                 },
1700         };
1701         u32 bitrate;
1702         /* default is 1 MHz index */
1703         int idx = 0;
1704
1705         if (rate->mcs >= 11)
1706                 goto warn;
1707
1708         switch (rate->bw) {
1709         case RATE_INFO_BW_16:
1710                 idx = 4;
1711                 break;
1712         case RATE_INFO_BW_8:
1713                 idx = 3;
1714                 break;
1715         case RATE_INFO_BW_4:
1716                 idx = 2;
1717                 break;
1718         case RATE_INFO_BW_2:
1719                 idx = 1;
1720                 break;
1721         case RATE_INFO_BW_1:
1722                 idx = 0;
1723                 break;
1724         case RATE_INFO_BW_5:
1725         case RATE_INFO_BW_10:
1726         case RATE_INFO_BW_20:
1727         case RATE_INFO_BW_40:
1728         case RATE_INFO_BW_80:
1729         case RATE_INFO_BW_160:
1730         default:
1731                 goto warn;
1732         }
1733
1734         bitrate = base[idx][rate->mcs];
1735         bitrate *= rate->nss;
1736
1737         if (rate->flags & RATE_INFO_FLAGS_SHORT_GI)
1738                 bitrate = (bitrate / 9) * 10;
1739         /* do NOT round down here */
1740         return (bitrate + 50000) / 100000;
1741 warn:
1742         WARN_ONCE(1, "invalid rate bw=%d, mcs=%d, nss=%d\n",
1743                   rate->bw, rate->mcs, rate->nss);
1744         return 0;
1745 }
1746
1747 u32 cfg80211_calculate_bitrate(struct rate_info *rate)
1748 {
1749         if (rate->flags & RATE_INFO_FLAGS_MCS)
1750                 return cfg80211_calculate_bitrate_ht(rate);
1751         if (rate->flags & RATE_INFO_FLAGS_DMG)
1752                 return cfg80211_calculate_bitrate_dmg(rate);
1753         if (rate->flags & RATE_INFO_FLAGS_EXTENDED_SC_DMG)
1754                 return cfg80211_calculate_bitrate_extended_sc_dmg(rate);
1755         if (rate->flags & RATE_INFO_FLAGS_EDMG)
1756                 return cfg80211_calculate_bitrate_edmg(rate);
1757         if (rate->flags & RATE_INFO_FLAGS_VHT_MCS)
1758                 return cfg80211_calculate_bitrate_vht(rate);
1759         if (rate->flags & RATE_INFO_FLAGS_HE_MCS)
1760                 return cfg80211_calculate_bitrate_he(rate);
1761         if (rate->flags & RATE_INFO_FLAGS_EHT_MCS)
1762                 return cfg80211_calculate_bitrate_eht(rate);
1763         if (rate->flags & RATE_INFO_FLAGS_S1G_MCS)
1764                 return cfg80211_calculate_bitrate_s1g(rate);
1765
1766         return rate->legacy;
1767 }
1768 EXPORT_SYMBOL(cfg80211_calculate_bitrate);
1769
1770 int cfg80211_get_p2p_attr(const u8 *ies, unsigned int len,
1771                           enum ieee80211_p2p_attr_id attr,
1772                           u8 *buf, unsigned int bufsize)
1773 {
1774         u8 *out = buf;
1775         u16 attr_remaining = 0;
1776         bool desired_attr = false;
1777         u16 desired_len = 0;
1778
1779         while (len > 0) {
1780                 unsigned int iedatalen;
1781                 unsigned int copy;
1782                 const u8 *iedata;
1783
1784                 if (len < 2)
1785                         return -EILSEQ;
1786                 iedatalen = ies[1];
1787                 if (iedatalen + 2 > len)
1788                         return -EILSEQ;
1789
1790                 if (ies[0] != WLAN_EID_VENDOR_SPECIFIC)
1791                         goto cont;
1792
1793                 if (iedatalen < 4)
1794                         goto cont;
1795
1796                 iedata = ies + 2;
1797
1798                 /* check WFA OUI, P2P subtype */
1799                 if (iedata[0] != 0x50 || iedata[1] != 0x6f ||
1800                     iedata[2] != 0x9a || iedata[3] != 0x09)
1801                         goto cont;
1802
1803                 iedatalen -= 4;
1804                 iedata += 4;
1805
1806                 /* check attribute continuation into this IE */
1807                 copy = min_t(unsigned int, attr_remaining, iedatalen);
1808                 if (copy && desired_attr) {
1809                         desired_len += copy;
1810                         if (out) {
1811                                 memcpy(out, iedata, min(bufsize, copy));
1812                                 out += min(bufsize, copy);
1813                                 bufsize -= min(bufsize, copy);
1814                         }
1815
1816
1817                         if (copy == attr_remaining)
1818                                 return desired_len;
1819                 }
1820
1821                 attr_remaining -= copy;
1822                 if (attr_remaining)
1823                         goto cont;
1824
1825                 iedatalen -= copy;
1826                 iedata += copy;
1827
1828                 while (iedatalen > 0) {
1829                         u16 attr_len;
1830
1831                         /* P2P attribute ID & size must fit */
1832                         if (iedatalen < 3)
1833                                 return -EILSEQ;
1834                         desired_attr = iedata[0] == attr;
1835                         attr_len = get_unaligned_le16(iedata + 1);
1836                         iedatalen -= 3;
1837                         iedata += 3;
1838
1839                         copy = min_t(unsigned int, attr_len, iedatalen);
1840
1841                         if (desired_attr) {
1842                                 desired_len += copy;
1843                                 if (out) {
1844                                         memcpy(out, iedata, min(bufsize, copy));
1845                                         out += min(bufsize, copy);
1846                                         bufsize -= min(bufsize, copy);
1847                                 }
1848
1849                                 if (copy == attr_len)
1850                                         return desired_len;
1851                         }
1852
1853                         iedata += copy;
1854                         iedatalen -= copy;
1855                         attr_remaining = attr_len - copy;
1856                 }
1857
1858  cont:
1859                 len -= ies[1] + 2;
1860                 ies += ies[1] + 2;
1861         }
1862
1863         if (attr_remaining && desired_attr)
1864                 return -EILSEQ;
1865
1866         return -ENOENT;
1867 }
1868 EXPORT_SYMBOL(cfg80211_get_p2p_attr);
1869
1870 static bool ieee80211_id_in_list(const u8 *ids, int n_ids, u8 id, bool id_ext)
1871 {
1872         int i;
1873
1874         /* Make sure array values are legal */
1875         if (WARN_ON(ids[n_ids - 1] == WLAN_EID_EXTENSION))
1876                 return false;
1877
1878         i = 0;
1879         while (i < n_ids) {
1880                 if (ids[i] == WLAN_EID_EXTENSION) {
1881                         if (id_ext && (ids[i + 1] == id))
1882                                 return true;
1883
1884                         i += 2;
1885                         continue;
1886                 }
1887
1888                 if (ids[i] == id && !id_ext)
1889                         return true;
1890
1891                 i++;
1892         }
1893         return false;
1894 }
1895
1896 static size_t skip_ie(const u8 *ies, size_t ielen, size_t pos)
1897 {
1898         /* we assume a validly formed IEs buffer */
1899         u8 len = ies[pos + 1];
1900
1901         pos += 2 + len;
1902
1903         /* the IE itself must have 255 bytes for fragments to follow */
1904         if (len < 255)
1905                 return pos;
1906
1907         while (pos < ielen && ies[pos] == WLAN_EID_FRAGMENT) {
1908                 len = ies[pos + 1];
1909                 pos += 2 + len;
1910         }
1911
1912         return pos;
1913 }
1914
1915 size_t ieee80211_ie_split_ric(const u8 *ies, size_t ielen,
1916                               const u8 *ids, int n_ids,
1917                               const u8 *after_ric, int n_after_ric,
1918                               size_t offset)
1919 {
1920         size_t pos = offset;
1921
1922         while (pos < ielen) {
1923                 u8 ext = 0;
1924
1925                 if (ies[pos] == WLAN_EID_EXTENSION)
1926                         ext = 2;
1927                 if ((pos + ext) >= ielen)
1928                         break;
1929
1930                 if (!ieee80211_id_in_list(ids, n_ids, ies[pos + ext],
1931                                           ies[pos] == WLAN_EID_EXTENSION))
1932                         break;
1933
1934                 if (ies[pos] == WLAN_EID_RIC_DATA && n_after_ric) {
1935                         pos = skip_ie(ies, ielen, pos);
1936
1937                         while (pos < ielen) {
1938                                 if (ies[pos] == WLAN_EID_EXTENSION)
1939                                         ext = 2;
1940                                 else
1941                                         ext = 0;
1942
1943                                 if ((pos + ext) >= ielen)
1944                                         break;
1945
1946                                 if (!ieee80211_id_in_list(after_ric,
1947                                                           n_after_ric,
1948                                                           ies[pos + ext],
1949                                                           ext == 2))
1950                                         pos = skip_ie(ies, ielen, pos);
1951                                 else
1952                                         break;
1953                         }
1954                 } else {
1955                         pos = skip_ie(ies, ielen, pos);
1956                 }
1957         }
1958
1959         return pos;
1960 }
1961 EXPORT_SYMBOL(ieee80211_ie_split_ric);
1962
1963 void ieee80211_fragment_element(struct sk_buff *skb, u8 *len_pos, u8 frag_id)
1964 {
1965         unsigned int elem_len;
1966
1967         if (!len_pos)
1968                 return;
1969
1970         elem_len = skb->data + skb->len - len_pos - 1;
1971
1972         while (elem_len > 255) {
1973                 /* this one is 255 */
1974                 *len_pos = 255;
1975                 /* remaining data gets smaller */
1976                 elem_len -= 255;
1977                 /* make space for the fragment ID/len in SKB */
1978                 skb_put(skb, 2);
1979                 /* shift back the remaining data to place fragment ID/len */
1980                 memmove(len_pos + 255 + 3, len_pos + 255 + 1, elem_len);
1981                 /* place the fragment ID */
1982                 len_pos += 255 + 1;
1983                 *len_pos = frag_id;
1984                 /* and point to fragment length to update later */
1985                 len_pos++;
1986         }
1987
1988         *len_pos = elem_len;
1989 }
1990 EXPORT_SYMBOL(ieee80211_fragment_element);
1991
1992 bool ieee80211_operating_class_to_band(u8 operating_class,
1993                                        enum nl80211_band *band)
1994 {
1995         switch (operating_class) {
1996         case 112:
1997         case 115 ... 127:
1998         case 128 ... 130:
1999                 *band = NL80211_BAND_5GHZ;
2000                 return true;
2001         case 131 ... 135:
2002         case 137:
2003                 *band = NL80211_BAND_6GHZ;
2004                 return true;
2005         case 81:
2006         case 82:
2007         case 83:
2008         case 84:
2009                 *band = NL80211_BAND_2GHZ;
2010                 return true;
2011         case 180:
2012                 *band = NL80211_BAND_60GHZ;
2013                 return true;
2014         }
2015
2016         return false;
2017 }
2018 EXPORT_SYMBOL(ieee80211_operating_class_to_band);
2019
2020 bool ieee80211_chandef_to_operating_class(struct cfg80211_chan_def *chandef,
2021                                           u8 *op_class)
2022 {
2023         u8 vht_opclass;
2024         u32 freq = chandef->center_freq1;
2025
2026         if (freq >= 2412 && freq <= 2472) {
2027                 if (chandef->width > NL80211_CHAN_WIDTH_40)
2028                         return false;
2029
2030                 /* 2.407 GHz, channels 1..13 */
2031                 if (chandef->width == NL80211_CHAN_WIDTH_40) {
2032                         if (freq > chandef->chan->center_freq)
2033                                 *op_class = 83; /* HT40+ */
2034                         else
2035                                 *op_class = 84; /* HT40- */
2036                 } else {
2037                         *op_class = 81;
2038                 }
2039
2040                 return true;
2041         }
2042
2043         if (freq == 2484) {
2044                 /* channel 14 is only for IEEE 802.11b */
2045                 if (chandef->width != NL80211_CHAN_WIDTH_20_NOHT)
2046                         return false;
2047
2048                 *op_class = 82; /* channel 14 */
2049                 return true;
2050         }
2051
2052         switch (chandef->width) {
2053         case NL80211_CHAN_WIDTH_80:
2054                 vht_opclass = 128;
2055                 break;
2056         case NL80211_CHAN_WIDTH_160:
2057                 vht_opclass = 129;
2058                 break;
2059         case NL80211_CHAN_WIDTH_80P80:
2060                 vht_opclass = 130;
2061                 break;
2062         case NL80211_CHAN_WIDTH_10:
2063         case NL80211_CHAN_WIDTH_5:
2064                 return false; /* unsupported for now */
2065         default:
2066                 vht_opclass = 0;
2067                 break;
2068         }
2069
2070         /* 5 GHz, channels 36..48 */
2071         if (freq >= 5180 && freq <= 5240) {
2072                 if (vht_opclass) {
2073                         *op_class = vht_opclass;
2074                 } else if (chandef->width == NL80211_CHAN_WIDTH_40) {
2075                         if (freq > chandef->chan->center_freq)
2076                                 *op_class = 116;
2077                         else
2078                                 *op_class = 117;
2079                 } else {
2080                         *op_class = 115;
2081                 }
2082
2083                 return true;
2084         }
2085
2086         /* 5 GHz, channels 52..64 */
2087         if (freq >= 5260 && freq <= 5320) {
2088                 if (vht_opclass) {
2089                         *op_class = vht_opclass;
2090                 } else if (chandef->width == NL80211_CHAN_WIDTH_40) {
2091                         if (freq > chandef->chan->center_freq)
2092                                 *op_class = 119;
2093                         else
2094                                 *op_class = 120;
2095                 } else {
2096                         *op_class = 118;
2097                 }
2098
2099                 return true;
2100         }
2101
2102         /* 5 GHz, channels 100..144 */
2103         if (freq >= 5500 && freq <= 5720) {
2104                 if (vht_opclass) {
2105                         *op_class = vht_opclass;
2106                 } else if (chandef->width == NL80211_CHAN_WIDTH_40) {
2107                         if (freq > chandef->chan->center_freq)
2108                                 *op_class = 122;
2109                         else
2110                                 *op_class = 123;
2111                 } else {
2112                         *op_class = 121;
2113                 }
2114
2115                 return true;
2116         }
2117
2118         /* 5 GHz, channels 149..169 */
2119         if (freq >= 5745 && freq <= 5845) {
2120                 if (vht_opclass) {
2121                         *op_class = vht_opclass;
2122                 } else if (chandef->width == NL80211_CHAN_WIDTH_40) {
2123                         if (freq > chandef->chan->center_freq)
2124                                 *op_class = 126;
2125                         else
2126                                 *op_class = 127;
2127                 } else if (freq <= 5805) {
2128                         *op_class = 124;
2129                 } else {
2130                         *op_class = 125;
2131                 }
2132
2133                 return true;
2134         }
2135
2136         /* 56.16 GHz, channel 1..4 */
2137         if (freq >= 56160 + 2160 * 1 && freq <= 56160 + 2160 * 6) {
2138                 if (chandef->width >= NL80211_CHAN_WIDTH_40)
2139                         return false;
2140
2141                 *op_class = 180;
2142                 return true;
2143         }
2144
2145         /* not supported yet */
2146         return false;
2147 }
2148 EXPORT_SYMBOL(ieee80211_chandef_to_operating_class);
2149
2150 static int cfg80211_wdev_bi(struct wireless_dev *wdev)
2151 {
2152         switch (wdev->iftype) {
2153         case NL80211_IFTYPE_AP:
2154         case NL80211_IFTYPE_P2P_GO:
2155                 WARN_ON(wdev->valid_links);
2156                 return wdev->links[0].ap.beacon_interval;
2157         case NL80211_IFTYPE_MESH_POINT:
2158                 return wdev->u.mesh.beacon_interval;
2159         case NL80211_IFTYPE_ADHOC:
2160                 return wdev->u.ibss.beacon_interval;
2161         default:
2162                 break;
2163         }
2164
2165         return 0;
2166 }
2167
2168 static void cfg80211_calculate_bi_data(struct wiphy *wiphy, u32 new_beacon_int,
2169                                        u32 *beacon_int_gcd,
2170                                        bool *beacon_int_different)
2171 {
2172         struct wireless_dev *wdev;
2173
2174         *beacon_int_gcd = 0;
2175         *beacon_int_different = false;
2176
2177         list_for_each_entry(wdev, &wiphy->wdev_list, list) {
2178                 int wdev_bi;
2179
2180                 /* this feature isn't supported with MLO */
2181                 if (wdev->valid_links)
2182                         continue;
2183
2184                 wdev_bi = cfg80211_wdev_bi(wdev);
2185
2186                 if (!wdev_bi)
2187                         continue;
2188
2189                 if (!*beacon_int_gcd) {
2190                         *beacon_int_gcd = wdev_bi;
2191                         continue;
2192                 }
2193
2194                 if (wdev_bi == *beacon_int_gcd)
2195                         continue;
2196
2197                 *beacon_int_different = true;
2198                 *beacon_int_gcd = gcd(*beacon_int_gcd, wdev_bi);
2199         }
2200
2201         if (new_beacon_int && *beacon_int_gcd != new_beacon_int) {
2202                 if (*beacon_int_gcd)
2203                         *beacon_int_different = true;
2204                 *beacon_int_gcd = gcd(*beacon_int_gcd, new_beacon_int);
2205         }
2206 }
2207
2208 int cfg80211_validate_beacon_int(struct cfg80211_registered_device *rdev,
2209                                  enum nl80211_iftype iftype, u32 beacon_int)
2210 {
2211         /*
2212          * This is just a basic pre-condition check; if interface combinations
2213          * are possible the driver must already be checking those with a call
2214          * to cfg80211_check_combinations(), in which case we'll validate more
2215          * through the cfg80211_calculate_bi_data() call and code in
2216          * cfg80211_iter_combinations().
2217          */
2218
2219         if (beacon_int < 10 || beacon_int > 10000)
2220                 return -EINVAL;
2221
2222         return 0;
2223 }
2224
2225 int cfg80211_iter_combinations(struct wiphy *wiphy,
2226                                struct iface_combination_params *params,
2227                                void (*iter)(const struct ieee80211_iface_combination *c,
2228                                             void *data),
2229                                void *data)
2230 {
2231         const struct ieee80211_regdomain *regdom;
2232         enum nl80211_dfs_regions region = 0;
2233         int i, j, iftype;
2234         int num_interfaces = 0;
2235         u32 used_iftypes = 0;
2236         u32 beacon_int_gcd;
2237         bool beacon_int_different;
2238
2239         /*
2240          * This is a bit strange, since the iteration used to rely only on
2241          * the data given by the driver, but here it now relies on context,
2242          * in form of the currently operating interfaces.
2243          * This is OK for all current users, and saves us from having to
2244          * push the GCD calculations into all the drivers.
2245          * In the future, this should probably rely more on data that's in
2246          * cfg80211 already - the only thing not would appear to be any new
2247          * interfaces (while being brought up) and channel/radar data.
2248          */
2249         cfg80211_calculate_bi_data(wiphy, params->new_beacon_int,
2250                                    &beacon_int_gcd, &beacon_int_different);
2251
2252         if (params->radar_detect) {
2253                 rcu_read_lock();
2254                 regdom = rcu_dereference(cfg80211_regdomain);
2255                 if (regdom)
2256                         region = regdom->dfs_region;
2257                 rcu_read_unlock();
2258         }
2259
2260         for (iftype = 0; iftype < NUM_NL80211_IFTYPES; iftype++) {
2261                 num_interfaces += params->iftype_num[iftype];
2262                 if (params->iftype_num[iftype] > 0 &&
2263                     !cfg80211_iftype_allowed(wiphy, iftype, 0, 1))
2264                         used_iftypes |= BIT(iftype);
2265         }
2266
2267         for (i = 0; i < wiphy->n_iface_combinations; i++) {
2268                 const struct ieee80211_iface_combination *c;
2269                 struct ieee80211_iface_limit *limits;
2270                 u32 all_iftypes = 0;
2271
2272                 c = &wiphy->iface_combinations[i];
2273
2274                 if (num_interfaces > c->max_interfaces)
2275                         continue;
2276                 if (params->num_different_channels > c->num_different_channels)
2277                         continue;
2278
2279                 limits = kmemdup(c->limits, sizeof(limits[0]) * c->n_limits,
2280                                  GFP_KERNEL);
2281                 if (!limits)
2282                         return -ENOMEM;
2283
2284                 for (iftype = 0; iftype < NUM_NL80211_IFTYPES; iftype++) {
2285                         if (cfg80211_iftype_allowed(wiphy, iftype, 0, 1))
2286                                 continue;
2287                         for (j = 0; j < c->n_limits; j++) {
2288                                 all_iftypes |= limits[j].types;
2289                                 if (!(limits[j].types & BIT(iftype)))
2290                                         continue;
2291                                 if (limits[j].max < params->iftype_num[iftype])
2292                                         goto cont;
2293                                 limits[j].max -= params->iftype_num[iftype];
2294                         }
2295                 }
2296
2297                 if (params->radar_detect !=
2298                         (c->radar_detect_widths & params->radar_detect))
2299                         goto cont;
2300
2301                 if (params->radar_detect && c->radar_detect_regions &&
2302                     !(c->radar_detect_regions & BIT(region)))
2303                         goto cont;
2304
2305                 /* Finally check that all iftypes that we're currently
2306                  * using are actually part of this combination. If they
2307                  * aren't then we can't use this combination and have
2308                  * to continue to the next.
2309                  */
2310                 if ((all_iftypes & used_iftypes) != used_iftypes)
2311                         goto cont;
2312
2313                 if (beacon_int_gcd) {
2314                         if (c->beacon_int_min_gcd &&
2315                             beacon_int_gcd < c->beacon_int_min_gcd)
2316                                 goto cont;
2317                         if (!c->beacon_int_min_gcd && beacon_int_different)
2318                                 goto cont;
2319                 }
2320
2321                 /* This combination covered all interface types and
2322                  * supported the requested numbers, so we're good.
2323                  */
2324
2325                 (*iter)(c, data);
2326  cont:
2327                 kfree(limits);
2328         }
2329
2330         return 0;
2331 }
2332 EXPORT_SYMBOL(cfg80211_iter_combinations);
2333
2334 static void
2335 cfg80211_iter_sum_ifcombs(const struct ieee80211_iface_combination *c,
2336                           void *data)
2337 {
2338         int *num = data;
2339         (*num)++;
2340 }
2341
2342 int cfg80211_check_combinations(struct wiphy *wiphy,
2343                                 struct iface_combination_params *params)
2344 {
2345         int err, num = 0;
2346
2347         err = cfg80211_iter_combinations(wiphy, params,
2348                                          cfg80211_iter_sum_ifcombs, &num);
2349         if (err)
2350                 return err;
2351         if (num == 0)
2352                 return -EBUSY;
2353
2354         return 0;
2355 }
2356 EXPORT_SYMBOL(cfg80211_check_combinations);
2357
2358 int ieee80211_get_ratemask(struct ieee80211_supported_band *sband,
2359                            const u8 *rates, unsigned int n_rates,
2360                            u32 *mask)
2361 {
2362         int i, j;
2363
2364         if (!sband)
2365                 return -EINVAL;
2366
2367         if (n_rates == 0 || n_rates > NL80211_MAX_SUPP_RATES)
2368                 return -EINVAL;
2369
2370         *mask = 0;
2371
2372         for (i = 0; i < n_rates; i++) {
2373                 int rate = (rates[i] & 0x7f) * 5;
2374                 bool found = false;
2375
2376                 for (j = 0; j < sband->n_bitrates; j++) {
2377                         if (sband->bitrates[j].bitrate == rate) {
2378                                 found = true;
2379                                 *mask |= BIT(j);
2380                                 break;
2381                         }
2382                 }
2383                 if (!found)
2384                         return -EINVAL;
2385         }
2386
2387         /*
2388          * mask must have at least one bit set here since we
2389          * didn't accept a 0-length rates array nor allowed
2390          * entries in the array that didn't exist
2391          */
2392
2393         return 0;
2394 }
2395
2396 unsigned int ieee80211_get_num_supported_channels(struct wiphy *wiphy)
2397 {
2398         enum nl80211_band band;
2399         unsigned int n_channels = 0;
2400
2401         for (band = 0; band < NUM_NL80211_BANDS; band++)
2402                 if (wiphy->bands[band])
2403                         n_channels += wiphy->bands[band]->n_channels;
2404
2405         return n_channels;
2406 }
2407 EXPORT_SYMBOL(ieee80211_get_num_supported_channels);
2408
2409 int cfg80211_get_station(struct net_device *dev, const u8 *mac_addr,
2410                          struct station_info *sinfo)
2411 {
2412         struct cfg80211_registered_device *rdev;
2413         struct wireless_dev *wdev;
2414
2415         wdev = dev->ieee80211_ptr;
2416         if (!wdev)
2417                 return -EOPNOTSUPP;
2418
2419         rdev = wiphy_to_rdev(wdev->wiphy);
2420         if (!rdev->ops->get_station)
2421                 return -EOPNOTSUPP;
2422
2423         memset(sinfo, 0, sizeof(*sinfo));
2424
2425         return rdev_get_station(rdev, dev, mac_addr, sinfo);
2426 }
2427 EXPORT_SYMBOL(cfg80211_get_station);
2428
2429 void cfg80211_free_nan_func(struct cfg80211_nan_func *f)
2430 {
2431         int i;
2432
2433         if (!f)
2434                 return;
2435
2436         kfree(f->serv_spec_info);
2437         kfree(f->srf_bf);
2438         kfree(f->srf_macs);
2439         for (i = 0; i < f->num_rx_filters; i++)
2440                 kfree(f->rx_filters[i].filter);
2441
2442         for (i = 0; i < f->num_tx_filters; i++)
2443                 kfree(f->tx_filters[i].filter);
2444
2445         kfree(f->rx_filters);
2446         kfree(f->tx_filters);
2447         kfree(f);
2448 }
2449 EXPORT_SYMBOL(cfg80211_free_nan_func);
2450
2451 bool cfg80211_does_bw_fit_range(const struct ieee80211_freq_range *freq_range,
2452                                 u32 center_freq_khz, u32 bw_khz)
2453 {
2454         u32 start_freq_khz, end_freq_khz;
2455
2456         start_freq_khz = center_freq_khz - (bw_khz / 2);
2457         end_freq_khz = center_freq_khz + (bw_khz / 2);
2458
2459         if (start_freq_khz >= freq_range->start_freq_khz &&
2460             end_freq_khz <= freq_range->end_freq_khz)
2461                 return true;
2462
2463         return false;
2464 }
2465
2466 int cfg80211_sinfo_alloc_tid_stats(struct station_info *sinfo, gfp_t gfp)
2467 {
2468         sinfo->pertid = kcalloc(IEEE80211_NUM_TIDS + 1,
2469                                 sizeof(*(sinfo->pertid)),
2470                                 gfp);
2471         if (!sinfo->pertid)
2472                 return -ENOMEM;
2473
2474         return 0;
2475 }
2476 EXPORT_SYMBOL(cfg80211_sinfo_alloc_tid_stats);
2477
2478 /* See IEEE 802.1H for LLC/SNAP encapsulation/decapsulation */
2479 /* Ethernet-II snap header (RFC1042 for most EtherTypes) */
2480 const unsigned char rfc1042_header[] __aligned(2) =
2481         { 0xaa, 0xaa, 0x03, 0x00, 0x00, 0x00 };
2482 EXPORT_SYMBOL(rfc1042_header);
2483
2484 /* Bridge-Tunnel header (for EtherTypes ETH_P_AARP and ETH_P_IPX) */
2485 const unsigned char bridge_tunnel_header[] __aligned(2) =
2486         { 0xaa, 0xaa, 0x03, 0x00, 0x00, 0xf8 };
2487 EXPORT_SYMBOL(bridge_tunnel_header);
2488
2489 /* Layer 2 Update frame (802.2 Type 1 LLC XID Update response) */
2490 struct iapp_layer2_update {
2491         u8 da[ETH_ALEN];        /* broadcast */
2492         u8 sa[ETH_ALEN];        /* STA addr */
2493         __be16 len;             /* 6 */
2494         u8 dsap;                /* 0 */
2495         u8 ssap;                /* 0 */
2496         u8 control;
2497         u8 xid_info[3];
2498 } __packed;
2499
2500 void cfg80211_send_layer2_update(struct net_device *dev, const u8 *addr)
2501 {
2502         struct iapp_layer2_update *msg;
2503         struct sk_buff *skb;
2504
2505         /* Send Level 2 Update Frame to update forwarding tables in layer 2
2506          * bridge devices */
2507
2508         skb = dev_alloc_skb(sizeof(*msg));
2509         if (!skb)
2510                 return;
2511         msg = skb_put(skb, sizeof(*msg));
2512
2513         /* 802.2 Type 1 Logical Link Control (LLC) Exchange Identifier (XID)
2514          * Update response frame; IEEE Std 802.2-1998, 5.4.1.2.1 */
2515
2516         eth_broadcast_addr(msg->da);
2517         ether_addr_copy(msg->sa, addr);
2518         msg->len = htons(6);
2519         msg->dsap = 0;
2520         msg->ssap = 0x01;       /* NULL LSAP, CR Bit: Response */
2521         msg->control = 0xaf;    /* XID response lsb.1111F101.
2522                                  * F=0 (no poll command; unsolicited frame) */
2523         msg->xid_info[0] = 0x81;        /* XID format identifier */
2524         msg->xid_info[1] = 1;   /* LLC types/classes: Type 1 LLC */
2525         msg->xid_info[2] = 0;   /* XID sender's receive window size (RW) */
2526
2527         skb->dev = dev;
2528         skb->protocol = eth_type_trans(skb, dev);
2529         memset(skb->cb, 0, sizeof(skb->cb));
2530         netif_rx(skb);
2531 }
2532 EXPORT_SYMBOL(cfg80211_send_layer2_update);
2533
2534 int ieee80211_get_vht_max_nss(struct ieee80211_vht_cap *cap,
2535                               enum ieee80211_vht_chanwidth bw,
2536                               int mcs, bool ext_nss_bw_capable,
2537                               unsigned int max_vht_nss)
2538 {
2539         u16 map = le16_to_cpu(cap->supp_mcs.rx_mcs_map);
2540         int ext_nss_bw;
2541         int supp_width;
2542         int i, mcs_encoding;
2543
2544         if (map == 0xffff)
2545                 return 0;
2546
2547         if (WARN_ON(mcs > 9 || max_vht_nss > 8))
2548                 return 0;
2549         if (mcs <= 7)
2550                 mcs_encoding = 0;
2551         else if (mcs == 8)
2552                 mcs_encoding = 1;
2553         else
2554                 mcs_encoding = 2;
2555
2556         if (!max_vht_nss) {
2557                 /* find max_vht_nss for the given MCS */
2558                 for (i = 7; i >= 0; i--) {
2559                         int supp = (map >> (2 * i)) & 3;
2560
2561                         if (supp == 3)
2562                                 continue;
2563
2564                         if (supp >= mcs_encoding) {
2565                                 max_vht_nss = i + 1;
2566                                 break;
2567                         }
2568                 }
2569         }
2570
2571         if (!(cap->supp_mcs.tx_mcs_map &
2572                         cpu_to_le16(IEEE80211_VHT_EXT_NSS_BW_CAPABLE)))
2573                 return max_vht_nss;
2574
2575         ext_nss_bw = le32_get_bits(cap->vht_cap_info,
2576                                    IEEE80211_VHT_CAP_EXT_NSS_BW_MASK);
2577         supp_width = le32_get_bits(cap->vht_cap_info,
2578                                    IEEE80211_VHT_CAP_SUPP_CHAN_WIDTH_MASK);
2579
2580         /* if not capable, treat ext_nss_bw as 0 */
2581         if (!ext_nss_bw_capable)
2582                 ext_nss_bw = 0;
2583
2584         /* This is invalid */
2585         if (supp_width == 3)
2586                 return 0;
2587
2588         /* This is an invalid combination so pretend nothing is supported */
2589         if (supp_width == 2 && (ext_nss_bw == 1 || ext_nss_bw == 2))
2590                 return 0;
2591
2592         /*
2593          * Cover all the special cases according to IEEE 802.11-2016
2594          * Table 9-250. All other cases are either factor of 1 or not
2595          * valid/supported.
2596          */
2597         switch (bw) {
2598         case IEEE80211_VHT_CHANWIDTH_USE_HT:
2599         case IEEE80211_VHT_CHANWIDTH_80MHZ:
2600                 if ((supp_width == 1 || supp_width == 2) &&
2601                     ext_nss_bw == 3)
2602                         return 2 * max_vht_nss;
2603                 break;
2604         case IEEE80211_VHT_CHANWIDTH_160MHZ:
2605                 if (supp_width == 0 &&
2606                     (ext_nss_bw == 1 || ext_nss_bw == 2))
2607                         return max_vht_nss / 2;
2608                 if (supp_width == 0 &&
2609                     ext_nss_bw == 3)
2610                         return (3 * max_vht_nss) / 4;
2611                 if (supp_width == 1 &&
2612                     ext_nss_bw == 3)
2613                         return 2 * max_vht_nss;
2614                 break;
2615         case IEEE80211_VHT_CHANWIDTH_80P80MHZ:
2616                 if (supp_width == 0 && ext_nss_bw == 1)
2617                         return 0; /* not possible */
2618                 if (supp_width == 0 &&
2619                     ext_nss_bw == 2)
2620                         return max_vht_nss / 2;
2621                 if (supp_width == 0 &&
2622                     ext_nss_bw == 3)
2623                         return (3 * max_vht_nss) / 4;
2624                 if (supp_width == 1 &&
2625                     ext_nss_bw == 0)
2626                         return 0; /* not possible */
2627                 if (supp_width == 1 &&
2628                     ext_nss_bw == 1)
2629                         return max_vht_nss / 2;
2630                 if (supp_width == 1 &&
2631                     ext_nss_bw == 2)
2632                         return (3 * max_vht_nss) / 4;
2633                 break;
2634         }
2635
2636         /* not covered or invalid combination received */
2637         return max_vht_nss;
2638 }
2639 EXPORT_SYMBOL(ieee80211_get_vht_max_nss);
2640
2641 bool cfg80211_iftype_allowed(struct wiphy *wiphy, enum nl80211_iftype iftype,
2642                              bool is_4addr, u8 check_swif)
2643
2644 {
2645         bool is_vlan = iftype == NL80211_IFTYPE_AP_VLAN;
2646
2647         switch (check_swif) {
2648         case 0:
2649                 if (is_vlan && is_4addr)
2650                         return wiphy->flags & WIPHY_FLAG_4ADDR_AP;
2651                 return wiphy->interface_modes & BIT(iftype);
2652         case 1:
2653                 if (!(wiphy->software_iftypes & BIT(iftype)) && is_vlan)
2654                         return wiphy->flags & WIPHY_FLAG_4ADDR_AP;
2655                 return wiphy->software_iftypes & BIT(iftype);
2656         default:
2657                 break;
2658         }
2659
2660         return false;
2661 }
2662 EXPORT_SYMBOL(cfg80211_iftype_allowed);
2663
2664 void cfg80211_remove_link(struct wireless_dev *wdev, unsigned int link_id)
2665 {
2666         struct cfg80211_registered_device *rdev = wiphy_to_rdev(wdev->wiphy);
2667
2668         lockdep_assert_wiphy(wdev->wiphy);
2669
2670         switch (wdev->iftype) {
2671         case NL80211_IFTYPE_AP:
2672         case NL80211_IFTYPE_P2P_GO:
2673                 cfg80211_stop_ap(rdev, wdev->netdev, link_id, true);
2674                 break;
2675         default:
2676                 /* per-link not relevant */
2677                 break;
2678         }
2679
2680         wdev->valid_links &= ~BIT(link_id);
2681
2682         rdev_del_intf_link(rdev, wdev, link_id);
2683
2684         eth_zero_addr(wdev->links[link_id].addr);
2685 }
2686
2687 void cfg80211_remove_links(struct wireless_dev *wdev)
2688 {
2689         unsigned int link_id;
2690
2691         /*
2692          * links are controlled by upper layers (userspace/cfg)
2693          * only for AP mode, so only remove them here for AP
2694          */
2695         if (wdev->iftype != NL80211_IFTYPE_AP)
2696                 return;
2697
2698         if (wdev->valid_links) {
2699                 for_each_valid_link(wdev, link_id)
2700                         cfg80211_remove_link(wdev, link_id);
2701         }
2702 }
2703
2704 int cfg80211_remove_virtual_intf(struct cfg80211_registered_device *rdev,
2705                                  struct wireless_dev *wdev)
2706 {
2707         cfg80211_remove_links(wdev);
2708
2709         return rdev_del_virtual_intf(rdev, wdev);
2710 }
2711
2712 const struct wiphy_iftype_ext_capab *
2713 cfg80211_get_iftype_ext_capa(struct wiphy *wiphy, enum nl80211_iftype type)
2714 {
2715         int i;
2716
2717         for (i = 0; i < wiphy->num_iftype_ext_capab; i++) {
2718                 if (wiphy->iftype_ext_capab[i].iftype == type)
2719                         return &wiphy->iftype_ext_capab[i];
2720         }
2721
2722         return NULL;
2723 }
2724 EXPORT_SYMBOL(cfg80211_get_iftype_ext_capa);