drivers/net/wireless/mwifiex/tdls.c

   1 /* Marvell Wireless LAN device driver: TDLS handling
   2  *
   3  * Copyright (C) 2014, Marvell International Ltd.
   4  *
   5  * This software file (the "File") is distributed by Marvell International
   6  * Ltd. under the terms of the GNU General Public License Version 2, June 1991
   7  * (the "License").  You may use, redistribute and/or modify this File in
   8  * accordance with the terms and conditions of the License, a copy of which
   9  * is available on the worldwide web at
  10  * http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
  11  *
  12  * THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE
  13  * IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE
  14  * ARE EXPRESSLY DISCLAIMED.  The License provides additional details about
  15  * this warranty disclaimer.
  16  */
  17
  18 #include "main.h"
  19
  20 /* This function appends rate TLV to scan config command. */
  21 static int
  22 mwifiex_tdls_append_rates_ie(struct mwifiex_private *priv,
  23                              struct sk_buff *skb)
  24 {
  25         u8 rates[MWIFIEX_SUPPORTED_RATES], *pos;
  26         u16 rates_size, supp_rates_size, ext_rates_size;
  27
  28         memset(rates, 0, sizeof(rates));
  29         rates_size = mwifiex_get_supported_rates(priv, rates);
  30
  31         supp_rates_size = min_t(u16, rates_size, MWIFIEX_TDLS_SUPPORTED_RATES);
  32
  33         if (skb_tailroom(skb) < rates_size + 4) {
  34                 dev_err(priv->adapter->dev,
  35                         "Insuffient space while adding rates\n");
  36                 return -ENOMEM;
  37         }
  38
  39         pos = skb_put(skb, supp_rates_size + 2);
  40         *pos++ = WLAN_EID_SUPP_RATES;
  41         *pos++ = supp_rates_size;
  42         memcpy(pos, rates, supp_rates_size);
  43
  44         if (rates_size > MWIFIEX_TDLS_SUPPORTED_RATES) {
  45                 ext_rates_size = rates_size - MWIFIEX_TDLS_SUPPORTED_RATES;
  46                 pos = skb_put(skb, ext_rates_size + 2);
  47                 *pos++ = WLAN_EID_EXT_SUPP_RATES;
  48                 *pos++ = ext_rates_size;
  49                 memcpy(pos, rates + MWIFIEX_TDLS_SUPPORTED_RATES,
  50                        ext_rates_size);
  51         }
  52
  53         return 0;
  54 }
  55
  56 static void mwifiex_tdls_add_ext_capab(struct sk_buff *skb)
  57 {
  58         struct ieee_types_extcap *extcap;
  59
  60         extcap = (void *)skb_put(skb, sizeof(struct ieee_types_extcap));
  61         extcap->ieee_hdr.element_id = WLAN_EID_EXT_CAPABILITY;
  62         extcap->ieee_hdr.len = 8;
  63         memset(extcap->ext_capab, 0, 8);
  64         extcap->ext_capab[4] |= WLAN_EXT_CAPA5_TDLS_ENABLED;
  65 }
  66
  67 static void mwifiex_tdls_add_qos_capab(struct sk_buff *skb)
  68 {
  69         u8 *pos = (void *)skb_put(skb, 3);
  70
  71         *pos++ = WLAN_EID_QOS_CAPA;
  72         *pos++ = 1;
  73         *pos++ = MWIFIEX_TDLS_DEF_QOS_CAPAB;
  74 }
  75
  76 static int mwifiex_prep_tdls_encap_data(struct mwifiex_private *priv,
  77                              u8 *peer, u8 action_code, u8 dialog_token,
  78                              u16 status_code, struct sk_buff *skb)
  79 {
  80         struct ieee80211_tdls_data *tf;
  81         int ret;
  82         u16 capab;
  83         struct ieee80211_ht_cap *ht_cap;
  84         u8 radio, *pos;
  85
  86         capab = priv->curr_bss_params.bss_descriptor.cap_info_bitmap;
  87
  88         tf = (void *)skb_put(skb, offsetof(struct ieee80211_tdls_data, u));
  89         memcpy(tf->da, peer, ETH_ALEN);
  90         memcpy(tf->sa, priv->curr_addr, ETH_ALEN);
  91         tf->ether_type = cpu_to_be16(ETH_P_TDLS);
  92         tf->payload_type = WLAN_TDLS_SNAP_RFTYPE;
  93
  94         switch (action_code) {
  95         case WLAN_TDLS_SETUP_REQUEST:
  96                 tf->category = WLAN_CATEGORY_TDLS;
  97                 tf->action_code = WLAN_TDLS_SETUP_REQUEST;
  98                 skb_put(skb, sizeof(tf->u.setup_req));
  99                 tf->u.setup_req.dialog_token = dialog_token;
 100                 tf->u.setup_req.capability = cpu_to_le16(capab);
 101                 ret = mwifiex_tdls_append_rates_ie(priv, skb);
 102                 if (ret) {
 103                         dev_kfree_skb_any(skb);
 104                         return ret;
 105                 }
 106
 107                 pos = (void *)skb_put(skb, sizeof(struct ieee80211_ht_cap) + 2);
 108                 *pos++ = WLAN_EID_HT_CAPABILITY;
 109                 *pos++ = sizeof(struct ieee80211_ht_cap);
 110                 ht_cap = (void *)pos;
 111                 radio = mwifiex_band_to_radio_type(priv->curr_bss_params.band);
 112                 ret = mwifiex_fill_cap_info(priv, radio, ht_cap);
 113                 if (ret) {
 114                         dev_kfree_skb_any(skb);
 115                         return ret;
 116                 }
 117
 118                 mwifiex_tdls_add_ext_capab(skb);
 119                 mwifiex_tdls_add_qos_capab(skb);
 120                 break;
 121
 122         case WLAN_TDLS_SETUP_RESPONSE:
 123                 tf->category = WLAN_CATEGORY_TDLS;
 124                 tf->action_code = WLAN_TDLS_SETUP_RESPONSE;
 125                 skb_put(skb, sizeof(tf->u.setup_resp));
 126                 tf->u.setup_resp.status_code = cpu_to_le16(status_code);
 127                 tf->u.setup_resp.dialog_token = dialog_token;
 128                 tf->u.setup_resp.capability = cpu_to_le16(capab);
 129                 ret = mwifiex_tdls_append_rates_ie(priv, skb);
 130                 if (ret) {
 131                         dev_kfree_skb_any(skb);
 132                         return ret;
 133                 }
 134
 135                 pos = (void *)skb_put(skb, sizeof(struct ieee80211_ht_cap) + 2);
 136                 *pos++ = WLAN_EID_HT_CAPABILITY;
 137                 *pos++ = sizeof(struct ieee80211_ht_cap);
 138                 ht_cap = (void *)pos;
 139                 radio = mwifiex_band_to_radio_type(priv->curr_bss_params.band);
 140                 ret = mwifiex_fill_cap_info(priv, radio, ht_cap);
 141                 if (ret) {
 142                         dev_kfree_skb_any(skb);
 143                         return ret;
 144                 }
 145
 146                 mwifiex_tdls_add_ext_capab(skb);
 147                 mwifiex_tdls_add_qos_capab(skb);
 148                 break;
 149
 150         case WLAN_TDLS_SETUP_CONFIRM:
 151                 tf->category = WLAN_CATEGORY_TDLS;
 152                 tf->action_code = WLAN_TDLS_SETUP_CONFIRM;
 153                 skb_put(skb, sizeof(tf->u.setup_cfm));
 154                 tf->u.setup_cfm.status_code = cpu_to_le16(status_code);
 155                 tf->u.setup_cfm.dialog_token = dialog_token;
 156                 break;
 157
 158         case WLAN_TDLS_TEARDOWN:
 159                 tf->category = WLAN_CATEGORY_TDLS;
 160                 tf->action_code = WLAN_TDLS_TEARDOWN;
 161                 skb_put(skb, sizeof(tf->u.teardown));
 162                 tf->u.teardown.reason_code = cpu_to_le16(status_code);
 163                 break;
 164
 165         case WLAN_TDLS_DISCOVERY_REQUEST:
 166                 tf->category = WLAN_CATEGORY_TDLS;
 167                 tf->action_code = WLAN_TDLS_DISCOVERY_REQUEST;
 168                 skb_put(skb, sizeof(tf->u.discover_req));
 169                 tf->u.discover_req.dialog_token = dialog_token;
 170                 break;
 171         default:
 172                 dev_err(priv->adapter->dev, "Unknown TDLS frame type.\n");
 173                 return -EINVAL;
 174         }
 175
 176         return 0;
 177 }
 178
 179 static void
 180 mwifiex_tdls_add_link_ie(struct sk_buff *skb, u8 *src_addr, u8 *peer, u8 *bssid)
 181 {
 182         struct ieee80211_tdls_lnkie *lnkid;
 183
 184         lnkid = (void *)skb_put(skb, sizeof(struct ieee80211_tdls_lnkie));
 185         lnkid->ie_type = WLAN_EID_LINK_ID;
 186         lnkid->ie_len = sizeof(struct ieee80211_tdls_lnkie) -
 187                         sizeof(struct ieee_types_header);
 188
 189         memcpy(lnkid->bssid, bssid, ETH_ALEN);
 190         memcpy(lnkid->init_sta, src_addr, ETH_ALEN);
 191         memcpy(lnkid->resp_sta, peer, ETH_ALEN);
 192 }
 193
 194 int mwifiex_send_tdls_data_frame(struct mwifiex_private *priv,
 195                                  u8 *peer, u8 action_code, u8 dialog_token,
 196                                  u16 status_code, const u8 *extra_ies,
 197                                  size_t extra_ies_len)
 198 {
 199         struct sk_buff *skb;
 200         struct mwifiex_txinfo *tx_info;
 201         struct timeval tv;
 202         int ret;
 203         u16 skb_len;
 204
 205         skb_len = MWIFIEX_MIN_DATA_HEADER_LEN +
 206                   max(sizeof(struct ieee80211_mgmt),
 207                       sizeof(struct ieee80211_tdls_data)) +
 208                   MWIFIEX_MGMT_FRAME_HEADER_SIZE +
 209                   MWIFIEX_SUPPORTED_RATES +
 210                   3 + /* Qos Info */
 211                   sizeof(struct ieee_types_extcap) +
 212                   sizeof(struct ieee80211_ht_cap) +
 213                   sizeof(struct ieee_types_bss_co_2040) +
 214                   sizeof(struct ieee80211_ht_operation) +
 215                   sizeof(struct ieee80211_tdls_lnkie) +
 216                   extra_ies_len;
 217
 218         skb = dev_alloc_skb(skb_len);
 219         if (!skb) {
 220                 dev_err(priv->adapter->dev,
 221                         "allocate skb failed for management frame\n");
 222                 return -ENOMEM;
 223         }
 224         skb_reserve(skb, MWIFIEX_MIN_DATA_HEADER_LEN);
 225
 226         switch (action_code) {
 227         case WLAN_TDLS_SETUP_REQUEST:
 228         case WLAN_TDLS_SETUP_CONFIRM:
 229         case WLAN_TDLS_TEARDOWN:
 230         case WLAN_TDLS_DISCOVERY_REQUEST:
 231                 ret = mwifiex_prep_tdls_encap_data(priv, peer, action_code,
 232                                                    dialog_token, status_code,
 233                                                    skb);
 234                 if (ret) {
 235                         dev_kfree_skb_any(skb);
 236                         return ret;
 237                 }
 238                 if (extra_ies_len)
 239                         memcpy(skb_put(skb, extra_ies_len), extra_ies,
 240                                extra_ies_len);
 241                 mwifiex_tdls_add_link_ie(skb, priv->curr_addr, peer,
 242                                          priv->cfg_bssid);
 243                 break;
 244         case WLAN_TDLS_SETUP_RESPONSE:
 245                 ret = mwifiex_prep_tdls_encap_data(priv, peer, action_code,
 246                                                    dialog_token, status_code,
 247                                                    skb);
 248                 if (ret) {
 249                         dev_kfree_skb_any(skb);
 250                         return ret;
 251                 }
 252                 if (extra_ies_len)
 253                         memcpy(skb_put(skb, extra_ies_len), extra_ies,
 254                                extra_ies_len);
 255                 mwifiex_tdls_add_link_ie(skb, peer, priv->curr_addr,
 256                                          priv->cfg_bssid);
 257                 break;
 258         }
 259
 260         switch (action_code) {
 261         case WLAN_TDLS_SETUP_REQUEST:
 262         case WLAN_TDLS_SETUP_RESPONSE:
 263                 skb->priority = MWIFIEX_PRIO_BK;
 264                 break;
 265         default:
 266                 skb->priority = MWIFIEX_PRIO_VI;
 267                 break;
 268         }
 269
 270         tx_info = MWIFIEX_SKB_TXCB(skb);
 271         tx_info->bss_num = priv->bss_num;
 272         tx_info->bss_type = priv->bss_type;
 273
 274         do_gettimeofday(&tv);
 275         skb->tstamp = timeval_to_ktime(tv);
 276         mwifiex_queue_tx_pkt(priv, skb);
 277
 278         return 0;
 279 }
 280
 281 static int
 282 mwifiex_construct_tdls_action_frame(struct mwifiex_private *priv, u8 *peer,
 283                                     u8 action_code, u8 dialog_token,
 284                                     u16 status_code, struct sk_buff *skb)
 285 {
 286         struct ieee80211_mgmt *mgmt;
 287         u8 bc_addr[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
 288         int ret;
 289         u16 capab;
 290         struct ieee80211_ht_cap *ht_cap;
 291         u8 radio, *pos;
 292
 293         capab = priv->curr_bss_params.bss_descriptor.cap_info_bitmap;
 294
 295         mgmt = (void *)skb_put(skb, offsetof(struct ieee80211_mgmt, u));
 296
 297         memset(mgmt, 0, 24);
 298         memcpy(mgmt->da, peer, ETH_ALEN);
 299         memcpy(mgmt->sa, priv->curr_addr, ETH_ALEN);
 300         memcpy(mgmt->bssid, priv->cfg_bssid, ETH_ALEN);
 301         mgmt->frame_control = cpu_to_le16(IEEE80211_FTYPE_MGMT |
 302                                           IEEE80211_STYPE_ACTION);
 303
 304         /* add address 4 */
 305         pos = skb_put(skb, ETH_ALEN);
 306
 307         switch (action_code) {
 308         case WLAN_PUB_ACTION_TDLS_DISCOVER_RES:
 309                 skb_put(skb, sizeof(mgmt->u.action.u.tdls_discover_resp) + 1);
 310                 mgmt->u.action.category = WLAN_CATEGORY_PUBLIC;
 311                 mgmt->u.action.u.tdls_discover_resp.action_code =
 312                                               WLAN_PUB_ACTION_TDLS_DISCOVER_RES;
 313                 mgmt->u.action.u.tdls_discover_resp.dialog_token =
 314                                                                    dialog_token;
 315                 mgmt->u.action.u.tdls_discover_resp.capability =
 316                                                              cpu_to_le16(capab);
 317                 /* move back for addr4 */
 318                 memmove(pos + ETH_ALEN, &mgmt->u.action.category,
 319                         sizeof(mgmt->u.action.u.tdls_discover_resp));
 320                 /* init address 4 */
 321                 memcpy(pos, bc_addr, ETH_ALEN);
 322
 323                 ret = mwifiex_tdls_append_rates_ie(priv, skb);
 324                 if (ret) {
 325                         dev_kfree_skb_any(skb);
 326                         return ret;
 327                 }
 328
 329                 pos = (void *)skb_put(skb, sizeof(struct ieee80211_ht_cap) + 2);
 330                 *pos++ = WLAN_EID_HT_CAPABILITY;
 331                 *pos++ = sizeof(struct ieee80211_ht_cap);
 332                 ht_cap = (void *)pos;
 333                 radio = mwifiex_band_to_radio_type(priv->curr_bss_params.band);
 334                 ret = mwifiex_fill_cap_info(priv, radio, ht_cap);
 335                 if (ret) {
 336                         dev_kfree_skb_any(skb);
 337                         return ret;
 338                 }
 339
 340                 mwifiex_tdls_add_ext_capab(skb);
 341                 mwifiex_tdls_add_qos_capab(skb);
 342                 break;
 343         default:
 344                 dev_err(priv->adapter->dev, "Unknown TDLS action frame type\n");
 345                 return -EINVAL;
 346         }
 347
 348         return 0;
 349 }
 350
 351 int mwifiex_send_tdls_action_frame(struct mwifiex_private *priv,
 352                                  u8 *peer, u8 action_code, u8 dialog_token,
 353                                  u16 status_code, const u8 *extra_ies,
 354                                  size_t extra_ies_len)
 355 {
 356         struct sk_buff *skb;
 357         struct mwifiex_txinfo *tx_info;
 358         struct timeval tv;
 359         u8 *pos;
 360         u32 pkt_type, tx_control;
 361         u16 pkt_len, skb_len;
 362
 363         skb_len = MWIFIEX_MIN_DATA_HEADER_LEN +
 364                   max(sizeof(struct ieee80211_mgmt),
 365                       sizeof(struct ieee80211_tdls_data)) +
 366                   MWIFIEX_MGMT_FRAME_HEADER_SIZE +
 367                   MWIFIEX_SUPPORTED_RATES +
 368                   sizeof(struct ieee_types_extcap) +
 369                   sizeof(struct ieee80211_ht_cap) +
 370                   sizeof(struct ieee_types_bss_co_2040) +
 371                   sizeof(struct ieee80211_ht_operation) +
 372                   sizeof(struct ieee80211_tdls_lnkie) +
 373                   extra_ies_len +
 374                   3 + /* Qos Info */
 375                   ETH_ALEN; /* Address4 */
 376
 377         skb = dev_alloc_skb(skb_len);
 378         if (!skb) {
 379                 dev_err(priv->adapter->dev,
 380                         "allocate skb failed for management frame\n");
 381                 return -ENOMEM;
 382         }
 383
 384         skb_reserve(skb, MWIFIEX_MIN_DATA_HEADER_LEN);
 385
 386         pkt_type = PKT_TYPE_MGMT;
 387         tx_control = 0;
 388         pos = skb_put(skb, MWIFIEX_MGMT_FRAME_HEADER_SIZE + sizeof(pkt_len));
 389         memset(pos, 0, MWIFIEX_MGMT_FRAME_HEADER_SIZE + sizeof(pkt_len));
 390         memcpy(pos, &pkt_type, sizeof(pkt_type));
 391         memcpy(pos + sizeof(pkt_type), &tx_control, sizeof(tx_control));
 392
 393         if (mwifiex_construct_tdls_action_frame(priv, peer, action_code,
 394                                                 dialog_token, status_code,
 395                                                 skb)) {
 396                 dev_kfree_skb_any(skb);
 397                 return -EINVAL;
 398         }
 399
 400         if (extra_ies_len)
 401                 memcpy(skb_put(skb, extra_ies_len), extra_ies, extra_ies_len);
 402
 403         /* the TDLS link IE is always added last we are the responder */
 404
 405         mwifiex_tdls_add_link_ie(skb, peer, priv->curr_addr,
 406                                  priv->cfg_bssid);
 407
 408         skb->priority = MWIFIEX_PRIO_VI;
 409
 410         tx_info = MWIFIEX_SKB_TXCB(skb);
 411         tx_info->bss_num = priv->bss_num;
 412         tx_info->bss_type = priv->bss_type;
 413         tx_info->flags |= MWIFIEX_BUF_FLAG_TDLS_PKT;
 414
 415         pkt_len = skb->len - MWIFIEX_MGMT_FRAME_HEADER_SIZE - sizeof(pkt_len);
 416         memcpy(skb->data + MWIFIEX_MGMT_FRAME_HEADER_SIZE, &pkt_len,
 417                sizeof(pkt_len));
 418         do_gettimeofday(&tv);
 419         skb->tstamp = timeval_to_ktime(tv);
 420         mwifiex_queue_tx_pkt(priv, skb);
 421
 422         return 0;
 423 }