drivers/net/wireless/eswin/fullmac/ecrnx_tdls.c

   1 /**
   2  ******************************************************************************
   3  *
   4  * @file ecrnx_tx.c
   5  *
   6  * Copyright (C) ESWIN 2015-2020
   7  *
   8  ******************************************************************************
   9  */
  10
  11 /**
  12  * INCLUDE FILES
  13  ******************************************************************************
  14  */
  15
  16 #include "ecrnx_tdls.h"
  17 #include "ecrnx_compat.h"
  18
  19 /**
  20  * FUNCTION DEFINITIONS
  21  ******************************************************************************
  22  */
  23
  24 static u16
  25 ecrnx_get_tdls_sta_capab(struct ecrnx_vif *ecrnx_vif, u16 status_code)
  26 {
  27     u16 capab = 0;
  28
  29     /* The capability will be 0 when sending a failure code */
  30     if (status_code != 0)
  31         return capab;
  32
  33     if (ecrnx_vif->sta.ap->band != NL80211_BAND_2GHZ)
  34         return capab;
  35
  36     capab |= WLAN_CAPABILITY_SHORT_SLOT_TIME;
  37     capab |= WLAN_CAPABILITY_SHORT_PREAMBLE;
  38
  39     return capab;
  40 }
  41
  42 static int
  43 ecrnx_tdls_prepare_encap_data(struct ecrnx_hw *ecrnx_hw, struct ecrnx_vif *ecrnx_vif,
  44                                  const u8 *peer, u8 action_code, u8 dialog_token,
  45                                  u16 status_code, struct sk_buff *skb)
  46 {
  47     struct ieee80211_tdls_data *tf;
  48     tf = (void *)skb_put(skb, sizeof(struct ieee80211_tdls_data) - sizeof(tf->u));
  49
  50     // set eth header
  51     memcpy(tf->da, peer, ETH_ALEN);
  52     memcpy(tf->sa, ecrnx_hw->wiphy->perm_addr, ETH_ALEN);
  53     tf->ether_type = cpu_to_be16(ETH_P_TDLS);
  54
  55     // set common TDLS info
  56     tf->payload_type = WLAN_TDLS_SNAP_RFTYPE;
  57     tf->category = WLAN_CATEGORY_TDLS;
  58     tf->action_code = action_code;
  59
  60     // set action specific TDLS info
  61     switch (action_code) {
  62     case WLAN_TDLS_SETUP_REQUEST:
  63         skb_put(skb, sizeof(tf->u.setup_req));
  64         tf->u.setup_req.dialog_token = dialog_token;
  65         tf->u.setup_req.capability =
  66                 cpu_to_le16(ecrnx_get_tdls_sta_capab(ecrnx_vif, status_code));
  67         break;
  68
  69     case WLAN_TDLS_SETUP_RESPONSE:
  70         skb_put(skb, sizeof(tf->u.setup_resp));
  71         tf->u.setup_resp.status_code = cpu_to_le16(status_code);
  72         tf->u.setup_resp.dialog_token = dialog_token;
  73         tf->u.setup_resp.capability =
  74                 cpu_to_le16(ecrnx_get_tdls_sta_capab(ecrnx_vif, status_code));
  75         break;
  76
  77     case WLAN_TDLS_SETUP_CONFIRM:
  78         skb_put(skb, sizeof(tf->u.setup_cfm));
  79         tf->u.setup_cfm.status_code = cpu_to_le16(status_code);
  80         tf->u.setup_cfm.dialog_token = dialog_token;
  81         break;
  82
  83     case WLAN_TDLS_TEARDOWN:
  84         skb_put(skb, sizeof(tf->u.teardown));
  85         tf->u.teardown.reason_code = cpu_to_le16(status_code);
  86         break;
  87
  88     case WLAN_TDLS_DISCOVERY_REQUEST:
  89         skb_put(skb, sizeof(tf->u.discover_req));
  90         tf->u.discover_req.dialog_token = dialog_token;
  91         break;
  92
  93     default:
  94         return -EINVAL;
  95     }
  96
  97     return 0;
  98 }
  99
 100 static int
 101 ecrnx_prep_tdls_direct(struct ecrnx_hw *ecrnx_hw, struct ecrnx_vif *ecrnx_vif,
 102                       const u8 *peer, u8 action_code, u8 dialog_token,
 103                       u16 status_code, struct sk_buff *skb)
 104 {
 105     struct ieee80211_mgmt *mgmt;
 106
 107     mgmt = (void *)skb_put(skb, 24);
 108     memset(mgmt, 0, 24);
 109     memcpy(mgmt->da, peer, ETH_ALEN);
 110     memcpy(mgmt->sa, ecrnx_hw->wiphy->perm_addr, ETH_ALEN);
 111     memcpy(mgmt->bssid, ecrnx_vif->sta.ap->mac_addr, ETH_ALEN);
 112
 113     mgmt->frame_control = cpu_to_le16(IEEE80211_FTYPE_MGMT |
 114                       IEEE80211_STYPE_ACTION);
 115
 116     switch (action_code) {
 117     case WLAN_PUB_ACTION_TDLS_DISCOVER_RES:
 118         skb_put(skb, 1 + sizeof(mgmt->u.action.u.tdls_discover_resp));
 119         mgmt->u.action.category = WLAN_CATEGORY_PUBLIC;
 120         mgmt->u.action.u.tdls_discover_resp.action_code = WLAN_PUB_ACTION_TDLS_DISCOVER_RES;
 121         mgmt->u.action.u.tdls_discover_resp.dialog_token = dialog_token;
 122         mgmt->u.action.u.tdls_discover_resp.capability =
 123             cpu_to_le16(ecrnx_get_tdls_sta_capab(ecrnx_vif, status_code));
 124         break;
 125     default:
 126         return -EINVAL;
 127     }
 128
 129     return 0;
 130 }
 131
 132 static int
 133 ecrnx_add_srates_ie(struct ecrnx_hw *ecrnx_hw, struct sk_buff *skb)
 134 {
 135     u8 i, rates, *pos;
 136     int rate;
 137     struct ieee80211_supported_band *ecrnx_band_2GHz = ecrnx_hw->wiphy->bands[NL80211_BAND_2GHZ];
 138
 139     rates = 8;
 140
 141     if (skb_tailroom(skb) < rates + 2)
 142         return -ENOMEM;
 143
 144     pos = skb_put(skb, rates + 2);
 145     *pos++ = WLAN_EID_SUPP_RATES;
 146     *pos++ = rates;
 147     for (i = 0; i < rates; i++) {
 148         rate = ecrnx_band_2GHz->bitrates[i].bitrate;
 149         rate = DIV_ROUND_UP(rate, 5);
 150         *pos++ = (u8)rate;
 151     }
 152
 153     return 0;
 154 }
 155
 156 static int
 157 ecrnx_add_ext_srates_ie(struct ecrnx_hw *ecrnx_hw, struct sk_buff *skb)
 158 {
 159     u8 i, exrates, *pos;
 160     int rate;
 161     struct ieee80211_supported_band *ecrnx_band_2GHz = ecrnx_hw->wiphy->bands[NL80211_BAND_2GHZ];
 162
 163     exrates = ecrnx_band_2GHz->n_bitrates - 8;
 164
 165     if (skb_tailroom(skb) < exrates + 2)
 166         return -ENOMEM;
 167
 168     pos = skb_put(skb, exrates + 2);
 169     *pos++ = WLAN_EID_EXT_SUPP_RATES;
 170     *pos++ = exrates;
 171     for (i = 8; i < (8+exrates); i++) {
 172         rate = ecrnx_band_2GHz->bitrates[i].bitrate;
 173         rate = DIV_ROUND_UP(rate, 5);
 174         *pos++ = (u8)rate;
 175     }
 176
 177     return 0;
 178 }
 179
 180 static void
 181 ecrnx_tdls_add_supp_channels(struct ecrnx_hw *ecrnx_hw, struct sk_buff *skb)
 182 {
 183     /*
 184      * Add possible channels for TDLS. These are channels that are allowed
 185      * to be active.
 186      */
 187     u8 subband_cnt = 0;
 188     u8 *pos_subband;
 189     u8 *pos = skb_put(skb, 2);
 190     struct ieee80211_supported_band *ecrnx_band_2GHz = ecrnx_hw->wiphy->bands[NL80211_BAND_2GHZ];
 191 #ifdef CONFIG_ECRNX_5G
 192     struct ieee80211_supported_band *ecrnx_band_5GHz = ecrnx_hw->wiphy->bands[NL80211_BAND_5GHZ];
 193 #endif
 194
 195     *pos++ = WLAN_EID_SUPPORTED_CHANNELS;
 196
 197     /*
 198      * 5GHz and 2GHz channels numbers can overlap. Ignore this for now, as
 199      * this doesn't happen in real world scenarios.
 200      */
 201
 202     /* 2GHz, with 5MHz spacing */
 203     pos_subband = skb_put(skb, 2);
 204     if (ecrnx_band_2GHz->n_channels > 0)
 205     {
 206         *pos_subband++ = ieee80211_frequency_to_channel(ecrnx_band_2GHz->channels[0].center_freq);
 207         *pos_subband++ = ecrnx_band_2GHz->n_channels;
 208         subband_cnt++;
 209     }
 210
 211 #ifdef CONFIG_ECRNX_5G
 212     /* 5GHz, with 20MHz spacing */
 213     pos_subband = skb_put(skb, 2);
 214     if (ecrnx_band_5GHz->n_channels > 0)
 215     {
 216         *pos_subband++ = ieee80211_frequency_to_channel(ecrnx_band_5GHz->channels[0].center_freq);
 217         *pos_subband++ = ecrnx_band_5GHz->n_channels;
 218         subband_cnt++;
 219     }
 220 #endif
 221
 222     /* length */
 223     *pos = 2 * subband_cnt;
 224 }
 225
 226 static void
 227 ecrnx_tdls_add_ext_capab(struct ecrnx_hw *ecrnx_hw, struct sk_buff *skb)
 228 {
 229     u8 *pos = (void *)skb_put(skb, 7);
 230     bool chan_switch = ecrnx_hw->wiphy->features &
 231                NL80211_FEATURE_TDLS_CHANNEL_SWITCH;
 232
 233     *pos++ = WLAN_EID_EXT_CAPABILITY;
 234     *pos++ = 5; /* len */
 235     *pos++ = 0x0;
 236     *pos++ = 0x0;
 237     *pos++ = 0x0;
 238     *pos++ = WLAN_EXT_CAPA4_TDLS_BUFFER_STA |
 239              (chan_switch ? WLAN_EXT_CAPA4_TDLS_CHAN_SWITCH : 0);
 240     *pos++ = WLAN_EXT_CAPA5_TDLS_ENABLED;
 241 }
 242
 243 static void
 244 ecrnx_add_wmm_info_ie(struct sk_buff *skb, u8 qosinfo)
 245 {
 246     u8 *pos = (void *)skb_put(skb, 9);
 247
 248     *pos++ = WLAN_EID_VENDOR_SPECIFIC;
 249     *pos++ = 7; /* len */
 250     *pos++ = 0x00; /* Microsoft OUI 00:50:F2 */
 251     *pos++ = 0x50;
 252     *pos++ = 0xf2;
 253     *pos++ = 2; /* WME */
 254     *pos++ = 0; /* WME info */
 255     *pos++ = 1; /* WME ver */
 256     *pos++ = qosinfo; /* U-APSD no in use */
 257 }
 258
 259 /* translate numbering in the WMM parameter IE to the mac80211 notation */
 260 static u8 ecrnx_ac_from_wmm(int ac)
 261 {
 262         switch (ac) {
 263         case 0:
 264                 return AC_BE;
 265         case 1:
 266                 return AC_BK;
 267         case 2:
 268                 return AC_VI;
 269         case 3:
 270                 return AC_VO;
 271     default:
 272         WARN_ON_ONCE(1);
 273         }
 274         return -1;
 275 }
 276
 277 static void
 278 ecrnx_add_wmm_param_ie(struct sk_buff *skb, u8 acm_bits, u32 *ac_params)
 279 {
 280     struct ieee80211_wmm_param_ie *wmm;
 281     int i, j;
 282     u8 cw_min, cw_max;
 283     bool acm;
 284
 285     wmm = (void *)skb_put(skb, sizeof(struct ieee80211_wmm_param_ie));
 286     memset(wmm, 0, sizeof(*wmm));
 287
 288     wmm->element_id = WLAN_EID_VENDOR_SPECIFIC;
 289     wmm->len = sizeof(*wmm) - 2;
 290
 291     wmm->oui[0] = 0x00; /* Microsoft OUI 00:50:F2 */
 292     wmm->oui[1] = 0x50;
 293     wmm->oui[2] = 0xf2;
 294     wmm->oui_type = 2; /* WME */
 295     wmm->oui_subtype = 1; /* WME param */
 296     wmm->version = 1; /* WME ver */
 297     wmm->qos_info = 0; /* U-APSD not in use */
 298
 299     /*
 300      * Use the EDCA parameters defined for the BSS, or default if the AP
 301      * doesn't support it, as mandated by 802.11-2012 section 10.22.4
 302      */
 303     for (i = 0; i < AC_MAX; i++) {
 304         j = ecrnx_ac_from_wmm(i);
 305         cw_min = (ac_params[j] & 0xF0 ) >> 4;
 306         cw_max = (ac_params[j] & 0xF00 ) >> 8;
 307         acm = (acm_bits & (1 << j)) != 0;
 308
 309         wmm->ac[i].aci_aifsn = (i << 5) | (acm << 4) | (ac_params[j] & 0xF);
 310         wmm->ac[i].cw = (cw_max << 4) | cw_min;
 311         wmm->ac[i].txop_limit = (ac_params[j] & 0x0FFFF000 ) >> 12;
 312     }
 313 }
 314
 315 static void
 316 ecrnx_tdls_add_oper_classes(struct ecrnx_vif *ecrnx_vif, struct sk_buff *skb)
 317 {
 318     u8 *pos;
 319     u8 op_class;
 320     struct cfg80211_chan_def chan_def;
 321     struct ieee80211_channel chan;
 322
 323     chan.band = ecrnx_vif->sta.ap->band;
 324     chan.center_freq = ecrnx_vif->sta.ap->center_freq;
 325     chan_def.chan = &chan;
 326     chan_def.width = ecrnx_vif->sta.ap->width;
 327     chan_def.center_freq1 = ecrnx_vif->sta.ap->center_freq1;
 328     chan_def.center_freq2 = ecrnx_vif->sta.ap->center_freq2;
 329
 330     if (!ieee80211_chandef_to_operating_class(&chan_def, &op_class))
 331         return;
 332
 333     pos = skb_put(skb, 4);
 334     *pos++ = WLAN_EID_SUPPORTED_REGULATORY_CLASSES;
 335     *pos++ = 2; /* len */
 336
 337     // current op class
 338     *pos++ = op_class;
 339     *pos++ = op_class; /* give current operating class as alternate too */
 340
 341     // need to add 5GHz classes?
 342 }
 343
 344 static void
 345 ecrnx_ie_build_ht_cap(struct sk_buff *skb, struct ieee80211_sta_ht_cap *ht_cap,
 346                   u16 cap)
 347 {
 348     u8 *pos;
 349     __le16 tmp;
 350
 351     pos = skb_put(skb, sizeof(struct ieee80211_ht_cap) + 2);
 352     *pos++ = WLAN_EID_HT_CAPABILITY;
 353     *pos++ = sizeof(struct ieee80211_ht_cap);
 354     memset(pos, 0, sizeof(struct ieee80211_ht_cap));
 355
 356     /* capability flags */
 357     tmp = cpu_to_le16(cap);
 358     memcpy(pos, &tmp, sizeof(u16));
 359     pos += sizeof(u16);
 360
 361     /* AMPDU parameters */
 362     *pos++ = ht_cap->ampdu_factor |
 363          (ht_cap->ampdu_density <<
 364             IEEE80211_HT_AMPDU_PARM_DENSITY_SHIFT);
 365
 366     /* MCS set */
 367     memcpy(pos, &ht_cap->mcs, sizeof(ht_cap->mcs));
 368     pos += sizeof(ht_cap->mcs);
 369
 370     /* extended capabilities */
 371     pos += sizeof(__le16);
 372
 373     /* BF capabilities */
 374     pos += sizeof(__le32);
 375
 376     /* antenna selection */
 377     pos += sizeof(u8);
 378 }
 379
 380 static void
 381 ecrnx_ie_build_vht_cap(struct sk_buff *skb, struct ieee80211_sta_vht_cap *vht_cap,
 382                    u32 cap)
 383 {
 384     u8 *pos;
 385     __le32 tmp;
 386
 387     pos = skb_put(skb, 14);
 388
 389     *pos++ = WLAN_EID_VHT_CAPABILITY;
 390     *pos++ = sizeof(struct ieee80211_vht_cap);
 391     memset(pos, 0, sizeof(struct ieee80211_vht_cap));
 392
 393     /* capability flags */
 394     tmp = cpu_to_le32(cap);
 395     memcpy(pos, &tmp, sizeof(u32));
 396     pos += sizeof(u32);
 397
 398     /* VHT MCS set */
 399     memcpy(pos, &vht_cap->vht_mcs, sizeof(vht_cap->vht_mcs));
 400     pos += sizeof(vht_cap->vht_mcs);
 401 }
 402
 403 static void
 404 ecrnx_tdls_add_bss_coex_ie(struct sk_buff *skb)
 405 {
 406     u8 *pos = (void *)skb_put(skb, 3);
 407
 408     *pos++ = WLAN_EID_BSS_COEX_2040;
 409     *pos++ = 1; /* len */
 410
 411     *pos++ = WLAN_BSS_COEX_INFORMATION_REQUEST;
 412 }
 413
 414 static void
 415 ecrnx_tdls_add_link_ie(struct ecrnx_hw *ecrnx_hw, struct ecrnx_vif *ecrnx_vif,
 416                        struct sk_buff *skb, const u8 *peer,
 417                        bool initiator)
 418 {
 419     struct ieee80211_tdls_lnkie *lnkid;
 420     const u8 *init_addr, *rsp_addr;
 421
 422     if (initiator) {
 423         init_addr = ecrnx_hw->wiphy->perm_addr;
 424         rsp_addr = peer;
 425     } else {
 426         init_addr = peer;
 427         rsp_addr = ecrnx_hw->wiphy->perm_addr;
 428     }
 429
 430     lnkid = (void *)skb_put(skb, sizeof(struct ieee80211_tdls_lnkie));
 431
 432     lnkid->ie_type = WLAN_EID_LINK_ID;
 433     lnkid->ie_len = sizeof(struct ieee80211_tdls_lnkie) - 2;
 434
 435     memcpy(lnkid->bssid, ecrnx_vif->sta.ap->mac_addr, ETH_ALEN);
 436     memcpy(lnkid->init_sta, init_addr, ETH_ALEN);
 437     memcpy(lnkid->resp_sta, rsp_addr, ETH_ALEN);
 438 }
 439
 440 static void
 441 ecrnx_tdls_add_aid_ie(struct ecrnx_vif *ecrnx_vif, struct sk_buff *skb)
 442 {
 443     u8 *pos = (void *)skb_put(skb, 4);
 444
 445 #if LINUX_VERSION_CODE >= KERNEL_VERSION(3, 12, 0)
 446     *pos++ = WLAN_EID_AID;
 447 #else
 448     *pos++ = 197;
 449 #endif
 450     *pos++ = 2; /* len */
 451     *pos++ = ecrnx_vif->sta.ap->aid;
 452 }
 453
 454 static u8 *
 455 ecrnx_ie_build_ht_oper(struct ecrnx_hw *ecrnx_hw, struct ecrnx_vif *ecrnx_vif,
 456                           u8 *pos, struct ieee80211_sta_ht_cap *ht_cap,
 457                           u16 prot_mode)
 458 {
 459     struct ieee80211_ht_operation *ht_oper;
 460     /* Build HT Information */
 461     *pos++ = WLAN_EID_HT_OPERATION;
 462     *pos++ = sizeof(struct ieee80211_ht_operation);
 463     ht_oper = (struct ieee80211_ht_operation *)pos;
 464     ht_oper->primary_chan = ieee80211_frequency_to_channel(
 465                     ecrnx_vif->sta.ap->center_freq);
 466     switch (ecrnx_vif->sta.ap->width) {
 467     case NL80211_CHAN_WIDTH_160:
 468     case NL80211_CHAN_WIDTH_80P80:
 469     case NL80211_CHAN_WIDTH_80:
 470     case NL80211_CHAN_WIDTH_40:
 471         if (ecrnx_vif->sta.ap->center_freq1 > ecrnx_vif->sta.ap->center_freq)
 472             ht_oper->ht_param = IEEE80211_HT_PARAM_CHA_SEC_ABOVE;
 473         else
 474             ht_oper->ht_param = IEEE80211_HT_PARAM_CHA_SEC_BELOW;
 475         break;
 476     default:
 477         ht_oper->ht_param = IEEE80211_HT_PARAM_CHA_SEC_NONE;
 478         break;
 479     }
 480     if (ht_cap->cap & IEEE80211_HT_CAP_SUP_WIDTH_20_40 &&
 481         ecrnx_vif->sta.ap->width != NL80211_CHAN_WIDTH_20_NOHT &&
 482         ecrnx_vif->sta.ap->width != NL80211_CHAN_WIDTH_20)
 483         ht_oper->ht_param |= IEEE80211_HT_PARAM_CHAN_WIDTH_ANY;
 484
 485     ht_oper->operation_mode = cpu_to_le16(prot_mode);
 486     ht_oper->stbc_param = 0x0000;
 487
 488     /* It seems that Basic MCS set and Supported MCS set
 489        are identical for the first 10 bytes */
 490     memset(&ht_oper->basic_set, 0, 16);
 491     memcpy(&ht_oper->basic_set, &ht_cap->mcs, 10);
 492
 493     return pos + sizeof(struct ieee80211_ht_operation);
 494 }
 495
 496 static u8 *
 497 ecrnx_ie_build_vht_oper(struct ecrnx_hw *ecrnx_hw, struct ecrnx_vif *ecrnx_vif,
 498                           u8 *pos, struct ieee80211_sta_ht_cap *ht_cap,
 499                           u16 prot_mode)
 500 {
 501     struct ieee80211_vht_operation *vht_oper;
 502     /* Build HT Information */
 503     *pos++ = WLAN_EID_VHT_OPERATION;
 504     *pos++ = sizeof(struct ieee80211_vht_operation);
 505     vht_oper = (struct ieee80211_vht_operation *)pos;
 506
 507     switch (ecrnx_vif->sta.ap->width) {
 508     case NL80211_CHAN_WIDTH_80:
 509         vht_oper->chan_width = IEEE80211_VHT_CHANWIDTH_80MHZ; // Channel Width
 510         CCFS0(vht_oper) =
 511                 ieee80211_frequency_to_channel(ecrnx_vif->sta.ap->center_freq); // Channel Center Frequency Segment 0
 512         CCFS1(vht_oper) = 0; // Channel Center Frequency Segment 1 (N.A.)
 513         break;
 514     case NL80211_CHAN_WIDTH_160:
 515         vht_oper->chan_width = IEEE80211_VHT_CHANWIDTH_160MHZ; // Channel Width
 516         CCFS0(vht_oper) =
 517                 ieee80211_frequency_to_channel(ecrnx_vif->sta.ap->center_freq); // Channel Center Frequency Segment 0
 518         CCFS1(vht_oper) = 0; // Channel Center Frequency Segment 1 (N.A.)
 519         break;
 520     case NL80211_CHAN_WIDTH_80P80:
 521         vht_oper->chan_width = IEEE80211_VHT_CHANWIDTH_80P80MHZ; // Channel Width
 522         CCFS0(vht_oper) =
 523                 ieee80211_frequency_to_channel(ecrnx_vif->sta.ap->center_freq1); // Channel Center Frequency Segment 0
 524         CCFS1(vht_oper) =
 525                 ieee80211_frequency_to_channel(ecrnx_vif->sta.ap->center_freq2); // Channel Center Frequency Segment 1
 526         break;
 527     default:
 528         vht_oper->chan_width = IEEE80211_VHT_CHANWIDTH_USE_HT;
 529         CCFS0(vht_oper) = 0;
 530         CCFS1(vht_oper) = 0;
 531         break;
 532     }
 533
 534     vht_oper->basic_mcs_set = cpu_to_le16(ecrnx_hw->mod_params->mcs_map);
 535
 536     return pos + sizeof(struct ieee80211_vht_operation);
 537
 538 }
 539
 540 static void
 541 ecrnx_tdls_add_setup_start_ies(struct ecrnx_hw *ecrnx_hw, struct ecrnx_vif *ecrnx_vif,
 542                               struct sk_buff *skb, const u8 *peer,
 543                               u8 action_code, bool initiator,
 544                               const u8 *extra_ies, size_t extra_ies_len)
 545 {
 546     enum nl80211_band band = ecrnx_vif->sta.ap->band;
 547     struct ieee80211_supported_band *sband;
 548     struct ieee80211_sta_ht_cap ht_cap;
 549     struct ieee80211_sta_vht_cap vht_cap;
 550     size_t offset = 0, noffset;
 551     u8 *pos;
 552
 553     rcu_read_lock();
 554
 555     ecrnx_add_srates_ie(ecrnx_hw, skb);
 556     ecrnx_add_ext_srates_ie(ecrnx_hw, skb);
 557     ecrnx_tdls_add_supp_channels(ecrnx_hw, skb);
 558     ecrnx_tdls_add_ext_capab(ecrnx_hw, skb);
 559
 560     /* add the QoS element if we support it */
 561     if (/*local->hw.queues >= IEEE80211_NUM_ACS &&*/
 562         action_code != WLAN_PUB_ACTION_TDLS_DISCOVER_RES)
 563         ecrnx_add_wmm_info_ie(skb, 0); /* no U-APSD */
 564
 565     ecrnx_tdls_add_oper_classes(ecrnx_vif, skb);
 566
 567     /*
 568      * with TDLS we can switch channels, and HT-caps are not necessarily
 569      * the same on all bands. The specification limits the setup to a
 570      * single HT-cap, so use the current band for now.
 571      */
 572     sband = ecrnx_hw->wiphy->bands[band];
 573     memcpy(&ht_cap, &sband->ht_cap, sizeof(ht_cap));
 574     if (((action_code == WLAN_TDLS_SETUP_REQUEST) ||
 575          (action_code == WLAN_TDLS_SETUP_RESPONSE) ||
 576          (action_code == WLAN_PUB_ACTION_TDLS_DISCOVER_RES)) &&
 577          ht_cap.ht_supported /* (!sta || sta->sta.ht_cap.ht_supported)*/) {
 578         ecrnx_ie_build_ht_cap(skb, &ht_cap, ht_cap.cap);
 579     }
 580
 581     if (ht_cap.ht_supported &&
 582         (ht_cap.cap & IEEE80211_HT_CAP_SUP_WIDTH_20_40))
 583         ecrnx_tdls_add_bss_coex_ie(skb);
 584
 585     ecrnx_tdls_add_link_ie(ecrnx_hw, ecrnx_vif, skb, peer, initiator);
 586
 587     memcpy(&vht_cap, &sband->vht_cap, sizeof(vht_cap));
 588     if (vht_cap.vht_supported) {
 589         ecrnx_tdls_add_aid_ie(ecrnx_vif, skb);
 590         ecrnx_ie_build_vht_cap(skb, &vht_cap, vht_cap.cap);
 591         // Operating mode Notification (optional)
 592     }
 593
 594     /* add any remaining IEs */
 595     if (extra_ies_len) {
 596         noffset = extra_ies_len;
 597         pos = skb_put(skb, noffset - offset);
 598         memcpy(pos, extra_ies + offset, noffset - offset);
 599     }
 600
 601     rcu_read_unlock();
 602 }
 603
 604
 605 static void
 606 ecrnx_tdls_add_setup_cfm_ies(struct ecrnx_hw *ecrnx_hw, struct ecrnx_vif *ecrnx_vif,
 607                               struct sk_buff *skb, const u8 *peer, bool initiator,
 608                               const u8 *extra_ies, size_t extra_ies_len)
 609 {
 610     struct ieee80211_supported_band *sband;
 611     enum nl80211_band band = ecrnx_vif->sta.ap->band;
 612     struct ieee80211_sta_ht_cap ht_cap;
 613     struct ieee80211_sta_vht_cap vht_cap;
 614
 615     size_t offset = 0, noffset;
 616     struct ecrnx_sta *sta, *ap_sta;
 617     u8 *pos;
 618
 619     rcu_read_lock();
 620
 621     sta = ecrnx_get_sta(ecrnx_hw, peer);
 622     ap_sta = ecrnx_vif->sta.ap;
 623     if (WARN_ON_ONCE(!sta || !ap_sta)) {
 624         rcu_read_unlock();
 625         return;
 626     }
 627
 628     /* add the QoS param IE if both the peer and we support it */
 629     if (sta->qos)
 630         ecrnx_add_wmm_param_ie(skb, ap_sta->acm, ap_sta->ac_param);
 631
 632     /* if HT support is only added in TDLS, we need an HT-operation IE */
 633     sband = ecrnx_hw->wiphy->bands[band];
 634     memcpy(&ht_cap, &sband->ht_cap, sizeof(ht_cap));
 635     if (ht_cap.ht_supported && !ap_sta->ht && sta->ht) {
 636         pos = skb_put(skb, 2 + sizeof(struct ieee80211_ht_operation));
 637         /* send an empty HT operation IE */
 638         ecrnx_ie_build_ht_oper(ecrnx_hw, ecrnx_vif, pos, &ht_cap, 0);
 639     }
 640
 641     ecrnx_tdls_add_link_ie(ecrnx_hw, ecrnx_vif, skb, peer, initiator);
 642
 643     memcpy(&vht_cap, &sband->vht_cap, sizeof(vht_cap));
 644     if (vht_cap.vht_supported && !ap_sta->vht && sta->vht) {
 645         pos = skb_put(skb, 2 + sizeof(struct ieee80211_vht_operation));
 646         ecrnx_ie_build_vht_oper(ecrnx_hw, ecrnx_vif, pos, &ht_cap, 0);
 647         // Operating mode Notification (optional)
 648     }
 649
 650     /* add any remaining IEs */
 651     if (extra_ies_len) {
 652         noffset = extra_ies_len;
 653         pos = skb_put(skb, noffset - offset);
 654         memcpy(pos, extra_ies + offset, noffset - offset);
 655     }
 656
 657     rcu_read_unlock();
 658 }
 659
 660 static void
 661 ecrnx_tdls_add_ies(struct ecrnx_hw *ecrnx_hw, struct ecrnx_vif *ecrnx_vif,
 662                                    struct sk_buff *skb, const u8 *peer,
 663                                    u8 action_code, u16 status_code,
 664                                    bool initiator, const u8 *extra_ies,
 665                                    size_t extra_ies_len, u8 oper_class,
 666                                    struct cfg80211_chan_def *chandef)
 667 {
 668     switch (action_code) {
 669     case WLAN_TDLS_SETUP_REQUEST:
 670     case WLAN_TDLS_SETUP_RESPONSE:
 671     case WLAN_PUB_ACTION_TDLS_DISCOVER_RES:
 672         if (status_code == 0)
 673             ecrnx_tdls_add_setup_start_ies(ecrnx_hw, ecrnx_vif, skb, peer, action_code,
 674                                           initiator, extra_ies, extra_ies_len);
 675         break;
 676     case WLAN_TDLS_SETUP_CONFIRM:
 677         if (status_code == 0)
 678             ecrnx_tdls_add_setup_cfm_ies(ecrnx_hw, ecrnx_vif, skb, peer, initiator,
 679                                         extra_ies, extra_ies_len);
 680         break;
 681
 682     case WLAN_TDLS_TEARDOWN:
 683     case WLAN_TDLS_DISCOVERY_REQUEST:
 684         if (extra_ies_len)
 685             memcpy(skb_put(skb, extra_ies_len), extra_ies,
 686                    extra_ies_len);
 687         if (status_code == 0 || action_code == WLAN_TDLS_TEARDOWN)
 688             ecrnx_tdls_add_link_ie(ecrnx_hw, ecrnx_vif, skb, peer, initiator);
 689         break;
 690     }
 691 }
 692
 693 int
 694 ecrnx_tdls_send_mgmt_packet_data(struct ecrnx_hw *ecrnx_hw, struct ecrnx_vif *ecrnx_vif,
 695                          const u8 *peer, u8 action_code, u8 dialog_token,
 696                          u16 status_code, u32 peer_capability, bool initiator,
 697                          const u8 *extra_ies, size_t extra_ies_len, u8 oper_class,
 698                          struct cfg80211_chan_def *chandef)
 699 {
 700     struct sk_buff *skb;
 701     int ret = 0;
 702     struct ieee80211_supported_band *ecrnx_band_2GHz = ecrnx_hw->wiphy->bands[NL80211_BAND_2GHZ];
 703 #ifdef CONFIG_ECRNX_5G
 704     struct ieee80211_supported_band *ecrnx_band_5GHz = ecrnx_hw->wiphy->bands[NL80211_BAND_5GHZ];
 705 #endif
 706     skb = netdev_alloc_skb(ecrnx_vif->ndev,
 707               sizeof(struct ieee80211_tdls_data) + // ethhdr + TDLS info
 708               10 +  /* supported rates */
 709               6 +  /* extended supported rates */
 710             #ifdef CONFIG_ECRNX_5G
 711               (2 + ecrnx_band_2GHz->n_channels + ecrnx_band_5GHz->n_channels) + /* supported channels */
 712             #else
 713                           (2 + ecrnx_band_2GHz->n_channels) +
 714                         #endif
 715               sizeof(struct ieee_types_extcap) +
 716               sizeof(struct ieee80211_wmm_param_ie) +
 717               4 + /* oper classes */
 718               28 + //sizeof(struct ieee80211_ht_cap) +
 719               sizeof(struct ieee_types_bss_co_2040) +
 720               sizeof(struct ieee80211_tdls_lnkie) +
 721               (2 + sizeof(struct ieee80211_vht_cap)) +
 722               4 + /*AID*/
 723               (2 + sizeof(struct ieee80211_ht_operation)) +
 724               extra_ies_len);
 725
 726     if (!skb)
 727         return 0;
 728
 729     switch (action_code) {
 730     case WLAN_TDLS_SETUP_REQUEST:
 731     case WLAN_TDLS_SETUP_RESPONSE:
 732     case WLAN_TDLS_SETUP_CONFIRM:
 733     case WLAN_TDLS_TEARDOWN:
 734     case WLAN_TDLS_DISCOVERY_REQUEST:
 735         ret = ecrnx_tdls_prepare_encap_data(ecrnx_hw, ecrnx_vif, peer, action_code,
 736                                            dialog_token, status_code, skb);
 737         break;
 738
 739     case WLAN_PUB_ACTION_TDLS_DISCOVER_RES:
 740         ret = ecrnx_prep_tdls_direct(ecrnx_hw, ecrnx_vif, peer, action_code,
 741                                     dialog_token, status_code, skb);
 742         break;
 743
 744     default:
 745         ret = -ENOTSUPP;
 746         break;
 747     }
 748
 749     if (ret < 0)
 750         goto fail;
 751
 752     ecrnx_tdls_add_ies(ecrnx_hw, ecrnx_vif, skb, peer, action_code, status_code,
 753                       initiator, extra_ies, extra_ies_len, oper_class, chandef);
 754
 755     if (action_code == WLAN_PUB_ACTION_TDLS_DISCOVER_RES) {
 756         u64 cookie;
 757 #if (LINUX_VERSION_CODE >= KERNEL_VERSION(3, 14, 0))
 758         struct cfg80211_mgmt_tx_params params;
 759
 760         params.len = skb->len;
 761         params.buf = skb->data;
 762         ret = ecrnx_start_mgmt_xmit(ecrnx_vif, NULL, &params, false, &cookie);
 763 #else
 764         ret = ecrnx_start_mgmt_xmit(ecrnx_vif, NULL, NULL, false, 0, skb->data, skb->len, false, false, &cookie);
 765 #endif /* LINUX_VERSION_CODE >= KERNEL_VERSION(3, 14, 0) */
 766         return ret;
 767     }
 768
 769     switch (action_code) {
 770     case WLAN_TDLS_SETUP_REQUEST:
 771     case WLAN_TDLS_SETUP_RESPONSE:
 772     case WLAN_TDLS_SETUP_CONFIRM:
 773         skb->priority = 2;
 774         break;
 775     default:
 776         skb->priority = 5;
 777         break;
 778     }
 779
 780     ret = ecrnx_select_txq(ecrnx_vif, skb);
 781     ret = ecrnx_start_xmit(skb, ecrnx_vif->ndev);
 782
 783    return ret;
 784
 785 fail:
 786     dev_kfree_skb(skb);
 787     return ret;
 788 }