d38555fe42847399cb17f78ad2ef7937239adacc
[sfrench/cifs-2.6.git] / drivers / net / wireless / marvell / 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 #include "wmm.h"
20 #include "11n.h"
21 #include "11n_rxreorder.h"
22 #include "11ac.h"
23
24 #define TDLS_REQ_FIX_LEN      6
25 #define TDLS_RESP_FIX_LEN     8
26 #define TDLS_CONFIRM_FIX_LEN  6
27 #define MWIFIEX_TDLS_WMM_INFO_SIZE 7
28
29 static void mwifiex_restore_tdls_packets(struct mwifiex_private *priv,
30                                          const u8 *mac, u8 status)
31 {
32         struct mwifiex_ra_list_tbl *ra_list;
33         struct list_head *tid_list;
34         struct sk_buff *skb, *tmp;
35         struct mwifiex_txinfo *tx_info;
36         unsigned long flags;
37         u32 tid;
38         u8 tid_down;
39
40         mwifiex_dbg(priv->adapter, DATA, "%s: %pM\n", __func__, mac);
41         spin_lock_irqsave(&priv->wmm.ra_list_spinlock, flags);
42
43         skb_queue_walk_safe(&priv->tdls_txq, skb, tmp) {
44                 if (!ether_addr_equal(mac, skb->data))
45                         continue;
46
47                 __skb_unlink(skb, &priv->tdls_txq);
48                 tx_info = MWIFIEX_SKB_TXCB(skb);
49                 tid = skb->priority;
50                 tid_down = mwifiex_wmm_downgrade_tid(priv, tid);
51
52                 if (mwifiex_is_tdls_link_setup(status)) {
53                         ra_list = mwifiex_wmm_get_queue_raptr(priv, tid, mac);
54                         ra_list->tdls_link = true;
55                         tx_info->flags |= MWIFIEX_BUF_FLAG_TDLS_PKT;
56                 } else {
57                         tid_list = &priv->wmm.tid_tbl_ptr[tid_down].ra_list;
58                         ra_list = list_first_entry_or_null(tid_list,
59                                         struct mwifiex_ra_list_tbl, list);
60                         tx_info->flags &= ~MWIFIEX_BUF_FLAG_TDLS_PKT;
61                 }
62
63                 if (!ra_list) {
64                         mwifiex_write_data_complete(priv->adapter, skb, 0, -1);
65                         continue;
66                 }
67
68                 skb_queue_tail(&ra_list->skb_head, skb);
69
70                 ra_list->ba_pkt_count++;
71                 ra_list->total_pkt_count++;
72
73                 if (atomic_read(&priv->wmm.highest_queued_prio) <
74                                                        tos_to_tid_inv[tid_down])
75                         atomic_set(&priv->wmm.highest_queued_prio,
76                                    tos_to_tid_inv[tid_down]);
77
78                 atomic_inc(&priv->wmm.tx_pkts_queued);
79         }
80
81         spin_unlock_irqrestore(&priv->wmm.ra_list_spinlock, flags);
82         return;
83 }
84
85 static void mwifiex_hold_tdls_packets(struct mwifiex_private *priv,
86                                       const u8 *mac)
87 {
88         struct mwifiex_ra_list_tbl *ra_list;
89         struct list_head *ra_list_head;
90         struct sk_buff *skb, *tmp;
91         unsigned long flags;
92         int i;
93
94         mwifiex_dbg(priv->adapter, DATA, "%s: %pM\n", __func__, mac);
95         spin_lock_irqsave(&priv->wmm.ra_list_spinlock, flags);
96
97         for (i = 0; i < MAX_NUM_TID; i++) {
98                 if (!list_empty(&priv->wmm.tid_tbl_ptr[i].ra_list)) {
99                         ra_list_head = &priv->wmm.tid_tbl_ptr[i].ra_list;
100                         list_for_each_entry(ra_list, ra_list_head, list) {
101                                 skb_queue_walk_safe(&ra_list->skb_head, skb,
102                                                     tmp) {
103                                         if (!ether_addr_equal(mac, skb->data))
104                                                 continue;
105                                         __skb_unlink(skb, &ra_list->skb_head);
106                                         atomic_dec(&priv->wmm.tx_pkts_queued);
107                                         ra_list->total_pkt_count--;
108                                         skb_queue_tail(&priv->tdls_txq, skb);
109                                 }
110                         }
111                 }
112         }
113
114         spin_unlock_irqrestore(&priv->wmm.ra_list_spinlock, flags);
115         return;
116 }
117
118 /* This function appends rate TLV to scan config command. */
119 static int
120 mwifiex_tdls_append_rates_ie(struct mwifiex_private *priv,
121                              struct sk_buff *skb)
122 {
123         u8 rates[MWIFIEX_SUPPORTED_RATES], *pos;
124         u16 rates_size, supp_rates_size, ext_rates_size;
125
126         memset(rates, 0, sizeof(rates));
127         rates_size = mwifiex_get_supported_rates(priv, rates);
128
129         supp_rates_size = min_t(u16, rates_size, MWIFIEX_TDLS_SUPPORTED_RATES);
130
131         if (skb_tailroom(skb) < rates_size + 4) {
132                 mwifiex_dbg(priv->adapter, ERROR,
133                             "Insuffient space while adding rates\n");
134                 return -ENOMEM;
135         }
136
137         pos = skb_put(skb, supp_rates_size + 2);
138         *pos++ = WLAN_EID_SUPP_RATES;
139         *pos++ = supp_rates_size;
140         memcpy(pos, rates, supp_rates_size);
141
142         if (rates_size > MWIFIEX_TDLS_SUPPORTED_RATES) {
143                 ext_rates_size = rates_size - MWIFIEX_TDLS_SUPPORTED_RATES;
144                 pos = skb_put(skb, ext_rates_size + 2);
145                 *pos++ = WLAN_EID_EXT_SUPP_RATES;
146                 *pos++ = ext_rates_size;
147                 memcpy(pos, rates + MWIFIEX_TDLS_SUPPORTED_RATES,
148                        ext_rates_size);
149         }
150
151         return 0;
152 }
153
154 static void mwifiex_tdls_add_aid(struct mwifiex_private *priv,
155                                 struct sk_buff *skb)
156 {
157         struct ieee_types_assoc_rsp *assoc_rsp;
158         u8 *pos;
159
160         assoc_rsp = (struct ieee_types_assoc_rsp *)&priv->assoc_rsp_buf;
161         pos = (void *)skb_put(skb, 4);
162         *pos++ = WLAN_EID_AID;
163         *pos++ = 2;
164         memcpy(pos, &assoc_rsp->a_id, sizeof(assoc_rsp->a_id));
165
166         return;
167 }
168
169 static int mwifiex_tdls_add_vht_capab(struct mwifiex_private *priv,
170                                       struct sk_buff *skb)
171 {
172         struct ieee80211_vht_cap vht_cap;
173         u8 *pos;
174
175         pos = (void *)skb_put(skb, sizeof(struct ieee80211_vht_cap) + 2);
176         *pos++ = WLAN_EID_VHT_CAPABILITY;
177         *pos++ = sizeof(struct ieee80211_vht_cap);
178
179         memset(&vht_cap, 0, sizeof(struct ieee80211_vht_cap));
180
181         mwifiex_fill_vht_cap_tlv(priv, &vht_cap, priv->curr_bss_params.band);
182         memcpy(pos, &vht_cap, sizeof(vht_cap));
183
184         return 0;
185 }
186
187 static int
188 mwifiex_tdls_add_ht_oper(struct mwifiex_private *priv, const u8 *mac,
189                          u8 vht_enabled, struct sk_buff *skb)
190 {
191         struct ieee80211_ht_operation *ht_oper;
192         struct mwifiex_sta_node *sta_ptr;
193         struct mwifiex_bssdescriptor *bss_desc =
194                                         &priv->curr_bss_params.bss_descriptor;
195         u8 *pos;
196
197         sta_ptr = mwifiex_get_sta_entry(priv, mac);
198         if (unlikely(!sta_ptr)) {
199                 mwifiex_dbg(priv->adapter, ERROR,
200                             "TDLS peer station not found in list\n");
201                 return -1;
202         }
203
204         if (!(le16_to_cpu(sta_ptr->tdls_cap.ht_capb.cap_info))) {
205                 mwifiex_dbg(priv->adapter, WARN,
206                             "TDLS peer doesn't support ht capabilities\n");
207                 return 0;
208         }
209
210         pos = (void *)skb_put(skb, sizeof(struct ieee80211_ht_operation) + 2);
211         *pos++ = WLAN_EID_HT_OPERATION;
212         *pos++ = sizeof(struct ieee80211_ht_operation);
213         ht_oper = (void *)pos;
214
215         ht_oper->primary_chan = bss_desc->channel;
216
217         /* follow AP's channel bandwidth */
218         if (ISSUPP_CHANWIDTH40(priv->adapter->hw_dot_11n_dev_cap) &&
219             bss_desc->bcn_ht_cap &&
220             ISALLOWED_CHANWIDTH40(bss_desc->bcn_ht_oper->ht_param))
221                 ht_oper->ht_param = bss_desc->bcn_ht_oper->ht_param;
222
223         if (vht_enabled) {
224                 ht_oper->ht_param =
225                           mwifiex_get_sec_chan_offset(bss_desc->channel);
226                 ht_oper->ht_param |= BIT(2);
227         }
228
229         memcpy(&sta_ptr->tdls_cap.ht_oper, ht_oper,
230                sizeof(struct ieee80211_ht_operation));
231
232         return 0;
233 }
234
235 static int mwifiex_tdls_add_vht_oper(struct mwifiex_private *priv,
236                                      const u8 *mac, struct sk_buff *skb)
237 {
238         struct mwifiex_bssdescriptor *bss_desc;
239         struct ieee80211_vht_operation *vht_oper;
240         struct ieee80211_vht_cap *vht_cap, *ap_vht_cap = NULL;
241         struct mwifiex_sta_node *sta_ptr;
242         struct mwifiex_adapter *adapter = priv->adapter;
243         u8 supp_chwd_set, peer_supp_chwd_set;
244         u8 *pos, ap_supp_chwd_set, chan_bw;
245         u16 mcs_map_user, mcs_map_resp, mcs_map_result;
246         u16 mcs_user, mcs_resp, nss;
247         u32 usr_vht_cap_info;
248
249         bss_desc = &priv->curr_bss_params.bss_descriptor;
250
251         sta_ptr = mwifiex_get_sta_entry(priv, mac);
252         if (unlikely(!sta_ptr)) {
253                 mwifiex_dbg(adapter, ERROR,
254                             "TDLS peer station not found in list\n");
255                 return -1;
256         }
257
258         if (!(le32_to_cpu(sta_ptr->tdls_cap.vhtcap.vht_cap_info))) {
259                 mwifiex_dbg(adapter, WARN,
260                             "TDLS peer doesn't support vht capabilities\n");
261                 return 0;
262         }
263
264         if (!mwifiex_is_bss_in_11ac_mode(priv)) {
265                 if (sta_ptr->tdls_cap.extcap.ext_capab[7] &
266                    WLAN_EXT_CAPA8_TDLS_WIDE_BW_ENABLED) {
267                         mwifiex_dbg(adapter, WARN,
268                                     "TDLS peer doesn't support wider bandwidth\n");
269                         return 0;
270                 }
271         } else {
272                 ap_vht_cap = bss_desc->bcn_vht_cap;
273         }
274
275         pos = (void *)skb_put(skb, sizeof(struct ieee80211_vht_operation) + 2);
276         *pos++ = WLAN_EID_VHT_OPERATION;
277         *pos++ = sizeof(struct ieee80211_vht_operation);
278         vht_oper = (struct ieee80211_vht_operation *)pos;
279
280         if (bss_desc->bss_band & BAND_A)
281                 usr_vht_cap_info = adapter->usr_dot_11ac_dev_cap_a;
282         else
283                 usr_vht_cap_info = adapter->usr_dot_11ac_dev_cap_bg;
284
285         /* find the minimum bandwidth between AP/TDLS peers */
286         vht_cap = &sta_ptr->tdls_cap.vhtcap;
287         supp_chwd_set = GET_VHTCAP_CHWDSET(usr_vht_cap_info);
288         peer_supp_chwd_set =
289                          GET_VHTCAP_CHWDSET(le32_to_cpu(vht_cap->vht_cap_info));
290         supp_chwd_set = min_t(u8, supp_chwd_set, peer_supp_chwd_set);
291
292         /* We need check AP's bandwidth when TDLS_WIDER_BANDWIDTH is off */
293
294         if (ap_vht_cap && sta_ptr->tdls_cap.extcap.ext_capab[7] &
295             WLAN_EXT_CAPA8_TDLS_WIDE_BW_ENABLED) {
296                 ap_supp_chwd_set =
297                       GET_VHTCAP_CHWDSET(le32_to_cpu(ap_vht_cap->vht_cap_info));
298                 supp_chwd_set = min_t(u8, supp_chwd_set, ap_supp_chwd_set);
299         }
300
301         switch (supp_chwd_set) {
302         case IEEE80211_VHT_CHANWIDTH_80MHZ:
303                 vht_oper->chan_width = IEEE80211_VHT_CHANWIDTH_80MHZ;
304                 break;
305         case IEEE80211_VHT_CHANWIDTH_160MHZ:
306                 vht_oper->chan_width = IEEE80211_VHT_CHANWIDTH_160MHZ;
307                 break;
308         case IEEE80211_VHT_CHANWIDTH_80P80MHZ:
309                 vht_oper->chan_width = IEEE80211_VHT_CHANWIDTH_80P80MHZ;
310                 break;
311         default:
312                 vht_oper->chan_width = IEEE80211_VHT_CHANWIDTH_USE_HT;
313                 break;
314         }
315
316         mcs_map_user = GET_DEVRXMCSMAP(adapter->usr_dot_11ac_mcs_support);
317         mcs_map_resp = le16_to_cpu(vht_cap->supp_mcs.rx_mcs_map);
318         mcs_map_result = 0;
319
320         for (nss = 1; nss <= 8; nss++) {
321                 mcs_user = GET_VHTNSSMCS(mcs_map_user, nss);
322                 mcs_resp = GET_VHTNSSMCS(mcs_map_resp, nss);
323
324                 if ((mcs_user == IEEE80211_VHT_MCS_NOT_SUPPORTED) ||
325                     (mcs_resp == IEEE80211_VHT_MCS_NOT_SUPPORTED))
326                         SET_VHTNSSMCS(mcs_map_result, nss,
327                                       IEEE80211_VHT_MCS_NOT_SUPPORTED);
328                 else
329                         SET_VHTNSSMCS(mcs_map_result, nss,
330                                       min_t(u16, mcs_user, mcs_resp));
331         }
332
333         vht_oper->basic_mcs_set = cpu_to_le16(mcs_map_result);
334
335         switch (vht_oper->chan_width) {
336         case IEEE80211_VHT_CHANWIDTH_80MHZ:
337                 chan_bw = IEEE80211_VHT_CHANWIDTH_80MHZ;
338                 break;
339         case IEEE80211_VHT_CHANWIDTH_160MHZ:
340                 chan_bw = IEEE80211_VHT_CHANWIDTH_160MHZ;
341                 break;
342         case IEEE80211_VHT_CHANWIDTH_80P80MHZ:
343                 chan_bw = IEEE80211_VHT_CHANWIDTH_80MHZ;
344                 break;
345         default:
346                 chan_bw = IEEE80211_VHT_CHANWIDTH_USE_HT;
347                 break;
348         }
349         vht_oper->center_freq_seg0_idx =
350                         mwifiex_get_center_freq_index(priv, BAND_AAC,
351                                                       bss_desc->channel,
352                                                       chan_bw);
353
354         return 0;
355 }
356
357 static void mwifiex_tdls_add_ext_capab(struct mwifiex_private *priv,
358                                        struct sk_buff *skb)
359 {
360         struct ieee_types_extcap *extcap;
361
362         extcap = (void *)skb_put(skb, sizeof(struct ieee_types_extcap));
363         extcap->ieee_hdr.element_id = WLAN_EID_EXT_CAPABILITY;
364         extcap->ieee_hdr.len = 8;
365         memset(extcap->ext_capab, 0, 8);
366         extcap->ext_capab[4] |= WLAN_EXT_CAPA5_TDLS_ENABLED;
367         extcap->ext_capab[3] |= WLAN_EXT_CAPA4_TDLS_CHAN_SWITCH;
368
369         if (priv->adapter->is_hw_11ac_capable)
370                 extcap->ext_capab[7] |= WLAN_EXT_CAPA8_TDLS_WIDE_BW_ENABLED;
371 }
372
373 static void mwifiex_tdls_add_qos_capab(struct sk_buff *skb)
374 {
375         u8 *pos = (void *)skb_put(skb, 3);
376
377         *pos++ = WLAN_EID_QOS_CAPA;
378         *pos++ = 1;
379         *pos++ = MWIFIEX_TDLS_DEF_QOS_CAPAB;
380 }
381
382 static void
383 mwifiex_tdls_add_wmm_param_ie(struct mwifiex_private *priv, struct sk_buff *skb)
384 {
385         struct ieee80211_wmm_param_ie *wmm;
386         u8 ac_vi[] = {0x42, 0x43, 0x5e, 0x00};
387         u8 ac_vo[] = {0x62, 0x32, 0x2f, 0x00};
388         u8 ac_be[] = {0x03, 0xa4, 0x00, 0x00};
389         u8 ac_bk[] = {0x27, 0xa4, 0x00, 0x00};
390
391         wmm = skb_put_zero(skb, sizeof(*wmm));
392
393         wmm->element_id = WLAN_EID_VENDOR_SPECIFIC;
394         wmm->len = sizeof(*wmm) - 2;
395         wmm->oui[0] = 0x00; /* Microsoft OUI 00:50:F2 */
396         wmm->oui[1] = 0x50;
397         wmm->oui[2] = 0xf2;
398         wmm->oui_type = 2; /* WME */
399         wmm->oui_subtype = 1; /* WME param */
400         wmm->version = 1; /* WME ver */
401         wmm->qos_info = 0; /* U-APSD not in use */
402
403         /* use default WMM AC parameters for TDLS link*/
404         memcpy(&wmm->ac[0], ac_be, sizeof(ac_be));
405         memcpy(&wmm->ac[1], ac_bk, sizeof(ac_bk));
406         memcpy(&wmm->ac[2], ac_vi, sizeof(ac_vi));
407         memcpy(&wmm->ac[3], ac_vo, sizeof(ac_vo));
408 }
409
410 static void
411 mwifiex_add_wmm_info_ie(struct mwifiex_private *priv, struct sk_buff *skb,
412                         u8 qosinfo)
413 {
414         u8 *buf;
415
416         buf = (void *)skb_put(skb, MWIFIEX_TDLS_WMM_INFO_SIZE +
417                               sizeof(struct ieee_types_header));
418
419         *buf++ = WLAN_EID_VENDOR_SPECIFIC;
420         *buf++ = 7; /* len */
421         *buf++ = 0x00; /* Microsoft OUI 00:50:F2 */
422         *buf++ = 0x50;
423         *buf++ = 0xf2;
424         *buf++ = 2; /* WME */
425         *buf++ = 0; /* WME info */
426         *buf++ = 1; /* WME ver */
427         *buf++ = qosinfo; /* U-APSD no in use */
428 }
429
430 static void mwifiex_tdls_add_bss_co_2040(struct sk_buff *skb)
431 {
432         struct ieee_types_bss_co_2040 *bssco;
433
434         bssco = (void *)skb_put(skb, sizeof(struct ieee_types_bss_co_2040));
435         bssco->ieee_hdr.element_id = WLAN_EID_BSS_COEX_2040;
436         bssco->ieee_hdr.len = sizeof(struct ieee_types_bss_co_2040) -
437                               sizeof(struct ieee_types_header);
438         bssco->bss_2040co = 0x01;
439 }
440
441 static void mwifiex_tdls_add_supported_chan(struct sk_buff *skb)
442 {
443         struct ieee_types_generic *supp_chan;
444         u8 chan_supp[] = {1, 11};
445
446         supp_chan = (void *)skb_put(skb, (sizeof(struct ieee_types_header) +
447                                           sizeof(chan_supp)));
448         supp_chan->ieee_hdr.element_id = WLAN_EID_SUPPORTED_CHANNELS;
449         supp_chan->ieee_hdr.len = sizeof(chan_supp);
450         memcpy(supp_chan->data, chan_supp, sizeof(chan_supp));
451 }
452
453 static void mwifiex_tdls_add_oper_class(struct sk_buff *skb)
454 {
455         struct ieee_types_generic *reg_class;
456         u8 rc_list[] = {1,
457                 1, 2, 3, 4, 12, 22, 23, 24, 25, 27, 28, 29, 30, 32, 33};
458         reg_class = (void *)skb_put(skb, (sizeof(struct ieee_types_header) +
459                                           sizeof(rc_list)));
460         reg_class->ieee_hdr.element_id = WLAN_EID_SUPPORTED_REGULATORY_CLASSES;
461         reg_class->ieee_hdr.len = sizeof(rc_list);
462         memcpy(reg_class->data, rc_list, sizeof(rc_list));
463 }
464
465 static int mwifiex_prep_tdls_encap_data(struct mwifiex_private *priv,
466                                         const u8 *peer, u8 action_code,
467                                         u8 dialog_token,
468                                         u16 status_code, struct sk_buff *skb)
469 {
470         struct ieee80211_tdls_data *tf;
471         int ret;
472         u16 capab;
473         struct ieee80211_ht_cap *ht_cap;
474         u8 radio, *pos;
475
476         capab = priv->curr_bss_params.bss_descriptor.cap_info_bitmap;
477
478         tf = (void *)skb_put(skb, offsetof(struct ieee80211_tdls_data, u));
479         memcpy(tf->da, peer, ETH_ALEN);
480         memcpy(tf->sa, priv->curr_addr, ETH_ALEN);
481         tf->ether_type = cpu_to_be16(ETH_P_TDLS);
482         tf->payload_type = WLAN_TDLS_SNAP_RFTYPE;
483
484         switch (action_code) {
485         case WLAN_TDLS_SETUP_REQUEST:
486                 tf->category = WLAN_CATEGORY_TDLS;
487                 tf->action_code = WLAN_TDLS_SETUP_REQUEST;
488                 skb_put(skb, sizeof(tf->u.setup_req));
489                 tf->u.setup_req.dialog_token = dialog_token;
490                 tf->u.setup_req.capability = cpu_to_le16(capab);
491                 ret = mwifiex_tdls_append_rates_ie(priv, skb);
492                 if (ret) {
493                         dev_kfree_skb_any(skb);
494                         return ret;
495                 }
496
497                 pos = (void *)skb_put(skb, sizeof(struct ieee80211_ht_cap) + 2);
498                 *pos++ = WLAN_EID_HT_CAPABILITY;
499                 *pos++ = sizeof(struct ieee80211_ht_cap);
500                 ht_cap = (void *)pos;
501                 radio = mwifiex_band_to_radio_type(priv->curr_bss_params.band);
502                 ret = mwifiex_fill_cap_info(priv, radio, ht_cap);
503                 if (ret) {
504                         dev_kfree_skb_any(skb);
505                         return ret;
506                 }
507
508                 if (priv->adapter->is_hw_11ac_capable) {
509                         ret = mwifiex_tdls_add_vht_capab(priv, skb);
510                         if (ret) {
511                                 dev_kfree_skb_any(skb);
512                                 return ret;
513                         }
514                         mwifiex_tdls_add_aid(priv, skb);
515                 }
516
517                 mwifiex_tdls_add_ext_capab(priv, skb);
518                 mwifiex_tdls_add_bss_co_2040(skb);
519                 mwifiex_tdls_add_supported_chan(skb);
520                 mwifiex_tdls_add_oper_class(skb);
521                 mwifiex_add_wmm_info_ie(priv, skb, 0);
522                 break;
523
524         case WLAN_TDLS_SETUP_RESPONSE:
525                 tf->category = WLAN_CATEGORY_TDLS;
526                 tf->action_code = WLAN_TDLS_SETUP_RESPONSE;
527                 skb_put(skb, sizeof(tf->u.setup_resp));
528                 tf->u.setup_resp.status_code = cpu_to_le16(status_code);
529                 tf->u.setup_resp.dialog_token = dialog_token;
530                 tf->u.setup_resp.capability = cpu_to_le16(capab);
531                 ret = mwifiex_tdls_append_rates_ie(priv, skb);
532                 if (ret) {
533                         dev_kfree_skb_any(skb);
534                         return ret;
535                 }
536
537                 pos = (void *)skb_put(skb, sizeof(struct ieee80211_ht_cap) + 2);
538                 *pos++ = WLAN_EID_HT_CAPABILITY;
539                 *pos++ = sizeof(struct ieee80211_ht_cap);
540                 ht_cap = (void *)pos;
541                 radio = mwifiex_band_to_radio_type(priv->curr_bss_params.band);
542                 ret = mwifiex_fill_cap_info(priv, radio, ht_cap);
543                 if (ret) {
544                         dev_kfree_skb_any(skb);
545                         return ret;
546                 }
547
548                 if (priv->adapter->is_hw_11ac_capable) {
549                         ret = mwifiex_tdls_add_vht_capab(priv, skb);
550                         if (ret) {
551                                 dev_kfree_skb_any(skb);
552                                 return ret;
553                         }
554                         mwifiex_tdls_add_aid(priv, skb);
555                 }
556
557                 mwifiex_tdls_add_ext_capab(priv, skb);
558                 mwifiex_tdls_add_bss_co_2040(skb);
559                 mwifiex_tdls_add_supported_chan(skb);
560                 mwifiex_tdls_add_oper_class(skb);
561                 mwifiex_add_wmm_info_ie(priv, skb, 0);
562                 break;
563
564         case WLAN_TDLS_SETUP_CONFIRM:
565                 tf->category = WLAN_CATEGORY_TDLS;
566                 tf->action_code = WLAN_TDLS_SETUP_CONFIRM;
567                 skb_put(skb, sizeof(tf->u.setup_cfm));
568                 tf->u.setup_cfm.status_code = cpu_to_le16(status_code);
569                 tf->u.setup_cfm.dialog_token = dialog_token;
570
571                 mwifiex_tdls_add_wmm_param_ie(priv, skb);
572                 if (priv->adapter->is_hw_11ac_capable) {
573                         ret = mwifiex_tdls_add_vht_oper(priv, peer, skb);
574                         if (ret) {
575                                 dev_kfree_skb_any(skb);
576                                 return ret;
577                         }
578                         ret = mwifiex_tdls_add_ht_oper(priv, peer, 1, skb);
579                         if (ret) {
580                                 dev_kfree_skb_any(skb);
581                                 return ret;
582                         }
583                 } else {
584                         ret = mwifiex_tdls_add_ht_oper(priv, peer, 0, skb);
585                         if (ret) {
586                                 dev_kfree_skb_any(skb);
587                                 return ret;
588                         }
589                 }
590                 break;
591
592         case WLAN_TDLS_TEARDOWN:
593                 tf->category = WLAN_CATEGORY_TDLS;
594                 tf->action_code = WLAN_TDLS_TEARDOWN;
595                 skb_put(skb, sizeof(tf->u.teardown));
596                 tf->u.teardown.reason_code = cpu_to_le16(status_code);
597                 break;
598
599         case WLAN_TDLS_DISCOVERY_REQUEST:
600                 tf->category = WLAN_CATEGORY_TDLS;
601                 tf->action_code = WLAN_TDLS_DISCOVERY_REQUEST;
602                 skb_put(skb, sizeof(tf->u.discover_req));
603                 tf->u.discover_req.dialog_token = dialog_token;
604                 break;
605         default:
606                 mwifiex_dbg(priv->adapter, ERROR, "Unknown TDLS frame type.\n");
607                 return -EINVAL;
608         }
609
610         return 0;
611 }
612
613 static void
614 mwifiex_tdls_add_link_ie(struct sk_buff *skb, const u8 *src_addr,
615                          const u8 *peer, const u8 *bssid)
616 {
617         struct ieee80211_tdls_lnkie *lnkid;
618
619         lnkid = (void *)skb_put(skb, sizeof(struct ieee80211_tdls_lnkie));
620         lnkid->ie_type = WLAN_EID_LINK_ID;
621         lnkid->ie_len = sizeof(struct ieee80211_tdls_lnkie) -
622                         sizeof(struct ieee_types_header);
623
624         memcpy(lnkid->bssid, bssid, ETH_ALEN);
625         memcpy(lnkid->init_sta, src_addr, ETH_ALEN);
626         memcpy(lnkid->resp_sta, peer, ETH_ALEN);
627 }
628
629 int mwifiex_send_tdls_data_frame(struct mwifiex_private *priv, const u8 *peer,
630                                  u8 action_code, u8 dialog_token,
631                                  u16 status_code, const u8 *extra_ies,
632                                  size_t extra_ies_len)
633 {
634         struct sk_buff *skb;
635         struct mwifiex_txinfo *tx_info;
636         int ret;
637         u16 skb_len;
638
639         skb_len = MWIFIEX_MIN_DATA_HEADER_LEN +
640                   max(sizeof(struct ieee80211_mgmt),
641                       sizeof(struct ieee80211_tdls_data)) +
642                   MWIFIEX_MGMT_FRAME_HEADER_SIZE +
643                   MWIFIEX_SUPPORTED_RATES +
644                   3 + /* Qos Info */
645                   sizeof(struct ieee_types_extcap) +
646                   sizeof(struct ieee80211_ht_cap) +
647                   sizeof(struct ieee_types_bss_co_2040) +
648                   sizeof(struct ieee80211_ht_operation) +
649                   sizeof(struct ieee80211_tdls_lnkie) +
650                   (2 * (sizeof(struct ieee_types_header))) +
651                    MWIFIEX_SUPPORTED_CHANNELS +
652                    MWIFIEX_OPERATING_CLASSES +
653                   sizeof(struct ieee80211_wmm_param_ie) +
654                   extra_ies_len;
655
656         if (priv->adapter->is_hw_11ac_capable)
657                 skb_len += sizeof(struct ieee_types_vht_cap) +
658                            sizeof(struct ieee_types_vht_oper) +
659                            sizeof(struct ieee_types_aid);
660
661         skb = dev_alloc_skb(skb_len);
662         if (!skb) {
663                 mwifiex_dbg(priv->adapter, ERROR,
664                             "allocate skb failed for management frame\n");
665                 return -ENOMEM;
666         }
667         skb_reserve(skb, MWIFIEX_MIN_DATA_HEADER_LEN);
668
669         switch (action_code) {
670         case WLAN_TDLS_SETUP_REQUEST:
671         case WLAN_TDLS_SETUP_CONFIRM:
672         case WLAN_TDLS_TEARDOWN:
673         case WLAN_TDLS_DISCOVERY_REQUEST:
674                 ret = mwifiex_prep_tdls_encap_data(priv, peer, action_code,
675                                                    dialog_token, status_code,
676                                                    skb);
677                 if (ret) {
678                         dev_kfree_skb_any(skb);
679                         return ret;
680                 }
681                 if (extra_ies_len)
682                         skb_put_data(skb, extra_ies, extra_ies_len);
683                 mwifiex_tdls_add_link_ie(skb, priv->curr_addr, peer,
684                                          priv->cfg_bssid);
685                 break;
686         case WLAN_TDLS_SETUP_RESPONSE:
687                 ret = mwifiex_prep_tdls_encap_data(priv, peer, action_code,
688                                                    dialog_token, status_code,
689                                                    skb);
690                 if (ret) {
691                         dev_kfree_skb_any(skb);
692                         return ret;
693                 }
694                 if (extra_ies_len)
695                         skb_put_data(skb, extra_ies, extra_ies_len);
696                 mwifiex_tdls_add_link_ie(skb, peer, priv->curr_addr,
697                                          priv->cfg_bssid);
698                 break;
699         }
700
701         switch (action_code) {
702         case WLAN_TDLS_SETUP_REQUEST:
703         case WLAN_TDLS_SETUP_RESPONSE:
704                 skb->priority = MWIFIEX_PRIO_BK;
705                 break;
706         default:
707                 skb->priority = MWIFIEX_PRIO_VI;
708                 break;
709         }
710
711         tx_info = MWIFIEX_SKB_TXCB(skb);
712         memset(tx_info, 0, sizeof(*tx_info));
713         tx_info->bss_num = priv->bss_num;
714         tx_info->bss_type = priv->bss_type;
715
716         __net_timestamp(skb);
717         mwifiex_queue_tx_pkt(priv, skb);
718
719         /* Delay 10ms to make sure tdls setup confirm/teardown frame
720          * is received by peer
721         */
722         if (action_code == WLAN_TDLS_SETUP_CONFIRM ||
723             action_code == WLAN_TDLS_TEARDOWN)
724                 msleep_interruptible(10);
725
726         return 0;
727 }
728
729 static int
730 mwifiex_construct_tdls_action_frame(struct mwifiex_private *priv,
731                                     const u8 *peer,
732                                     u8 action_code, u8 dialog_token,
733                                     u16 status_code, struct sk_buff *skb)
734 {
735         struct ieee80211_mgmt *mgmt;
736         u8 bc_addr[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
737         int ret;
738         u16 capab;
739         struct ieee80211_ht_cap *ht_cap;
740         u8 radio, *pos;
741
742         capab = priv->curr_bss_params.bss_descriptor.cap_info_bitmap;
743
744         mgmt = (void *)skb_put(skb, offsetof(struct ieee80211_mgmt, u));
745
746         memset(mgmt, 0, 24);
747         memcpy(mgmt->da, peer, ETH_ALEN);
748         memcpy(mgmt->sa, priv->curr_addr, ETH_ALEN);
749         memcpy(mgmt->bssid, priv->cfg_bssid, ETH_ALEN);
750         mgmt->frame_control = cpu_to_le16(IEEE80211_FTYPE_MGMT |
751                                           IEEE80211_STYPE_ACTION);
752
753         /* add address 4 */
754         pos = skb_put(skb, ETH_ALEN);
755
756         switch (action_code) {
757         case WLAN_PUB_ACTION_TDLS_DISCOVER_RES:
758                 skb_put(skb, sizeof(mgmt->u.action.u.tdls_discover_resp) + 1);
759                 mgmt->u.action.category = WLAN_CATEGORY_PUBLIC;
760                 mgmt->u.action.u.tdls_discover_resp.action_code =
761                                               WLAN_PUB_ACTION_TDLS_DISCOVER_RES;
762                 mgmt->u.action.u.tdls_discover_resp.dialog_token =
763                                                                    dialog_token;
764                 mgmt->u.action.u.tdls_discover_resp.capability =
765                                                              cpu_to_le16(capab);
766                 /* move back for addr4 */
767                 memmove(pos + ETH_ALEN, &mgmt->u.action.category,
768                         sizeof(mgmt->u.action.u.tdls_discover_resp));
769                 /* init address 4 */
770                 memcpy(pos, bc_addr, ETH_ALEN);
771
772                 ret = mwifiex_tdls_append_rates_ie(priv, skb);
773                 if (ret) {
774                         dev_kfree_skb_any(skb);
775                         return ret;
776                 }
777
778                 pos = (void *)skb_put(skb, sizeof(struct ieee80211_ht_cap) + 2);
779                 *pos++ = WLAN_EID_HT_CAPABILITY;
780                 *pos++ = sizeof(struct ieee80211_ht_cap);
781                 ht_cap = (void *)pos;
782                 radio = mwifiex_band_to_radio_type(priv->curr_bss_params.band);
783                 ret = mwifiex_fill_cap_info(priv, radio, ht_cap);
784                 if (ret) {
785                         dev_kfree_skb_any(skb);
786                         return ret;
787                 }
788
789                 if (priv->adapter->is_hw_11ac_capable) {
790                         ret = mwifiex_tdls_add_vht_capab(priv, skb);
791                         if (ret) {
792                                 dev_kfree_skb_any(skb);
793                                 return ret;
794                         }
795                         mwifiex_tdls_add_aid(priv, skb);
796                 }
797
798                 mwifiex_tdls_add_ext_capab(priv, skb);
799                 mwifiex_tdls_add_bss_co_2040(skb);
800                 mwifiex_tdls_add_supported_chan(skb);
801                 mwifiex_tdls_add_qos_capab(skb);
802                 mwifiex_tdls_add_oper_class(skb);
803                 break;
804         default:
805                 mwifiex_dbg(priv->adapter, ERROR, "Unknown TDLS action frame type\n");
806                 return -EINVAL;
807         }
808
809         return 0;
810 }
811
812 int mwifiex_send_tdls_action_frame(struct mwifiex_private *priv, const u8 *peer,
813                                    u8 action_code, u8 dialog_token,
814                                    u16 status_code, const u8 *extra_ies,
815                                    size_t extra_ies_len)
816 {
817         struct sk_buff *skb;
818         struct mwifiex_txinfo *tx_info;
819         u8 *pos;
820         u32 pkt_type, tx_control;
821         u16 pkt_len, skb_len;
822
823         skb_len = MWIFIEX_MIN_DATA_HEADER_LEN +
824                   max(sizeof(struct ieee80211_mgmt),
825                       sizeof(struct ieee80211_tdls_data)) +
826                   MWIFIEX_MGMT_FRAME_HEADER_SIZE +
827                   MWIFIEX_SUPPORTED_RATES +
828                   sizeof(struct ieee_types_extcap) +
829                   sizeof(struct ieee80211_ht_cap) +
830                   sizeof(struct ieee_types_bss_co_2040) +
831                   sizeof(struct ieee80211_ht_operation) +
832                   sizeof(struct ieee80211_tdls_lnkie) +
833                   extra_ies_len +
834                   3 + /* Qos Info */
835                   ETH_ALEN; /* Address4 */
836
837         if (priv->adapter->is_hw_11ac_capable)
838                 skb_len += sizeof(struct ieee_types_vht_cap) +
839                            sizeof(struct ieee_types_vht_oper) +
840                            sizeof(struct ieee_types_aid);
841
842         skb = dev_alloc_skb(skb_len);
843         if (!skb) {
844                 mwifiex_dbg(priv->adapter, ERROR,
845                             "allocate skb failed for management frame\n");
846                 return -ENOMEM;
847         }
848
849         skb_reserve(skb, MWIFIEX_MIN_DATA_HEADER_LEN);
850
851         pkt_type = PKT_TYPE_MGMT;
852         tx_control = 0;
853         pos = skb_put_zero(skb,
854                            MWIFIEX_MGMT_FRAME_HEADER_SIZE + sizeof(pkt_len));
855         memcpy(pos, &pkt_type, sizeof(pkt_type));
856         memcpy(pos + sizeof(pkt_type), &tx_control, sizeof(tx_control));
857
858         if (mwifiex_construct_tdls_action_frame(priv, peer, action_code,
859                                                 dialog_token, status_code,
860                                                 skb)) {
861                 dev_kfree_skb_any(skb);
862                 return -EINVAL;
863         }
864
865         if (extra_ies_len)
866                 skb_put_data(skb, extra_ies, extra_ies_len);
867
868         /* the TDLS link IE is always added last we are the responder */
869
870         mwifiex_tdls_add_link_ie(skb, peer, priv->curr_addr,
871                                  priv->cfg_bssid);
872
873         skb->priority = MWIFIEX_PRIO_VI;
874
875         tx_info = MWIFIEX_SKB_TXCB(skb);
876         memset(tx_info, 0, sizeof(*tx_info));
877         tx_info->bss_num = priv->bss_num;
878         tx_info->bss_type = priv->bss_type;
879         tx_info->flags |= MWIFIEX_BUF_FLAG_TDLS_PKT;
880
881         pkt_len = skb->len - MWIFIEX_MGMT_FRAME_HEADER_SIZE - sizeof(pkt_len);
882         memcpy(skb->data + MWIFIEX_MGMT_FRAME_HEADER_SIZE, &pkt_len,
883                sizeof(pkt_len));
884         __net_timestamp(skb);
885         mwifiex_queue_tx_pkt(priv, skb);
886
887         return 0;
888 }
889
890 /* This function process tdls action frame from peer.
891  * Peer capabilities are stored into station node structure.
892  */
893 void mwifiex_process_tdls_action_frame(struct mwifiex_private *priv,
894                                        u8 *buf, int len)
895 {
896         struct mwifiex_sta_node *sta_ptr;
897         u8 *peer, *pos, *end;
898         u8 i, action, basic;
899         u16 cap = 0;
900         int ie_len = 0;
901
902         if (len < (sizeof(struct ethhdr) + 3))
903                 return;
904         if (*(buf + sizeof(struct ethhdr)) != WLAN_TDLS_SNAP_RFTYPE)
905                 return;
906         if (*(buf + sizeof(struct ethhdr) + 1) != WLAN_CATEGORY_TDLS)
907                 return;
908
909         peer = buf + ETH_ALEN;
910         action = *(buf + sizeof(struct ethhdr) + 2);
911         mwifiex_dbg(priv->adapter, DATA,
912                     "rx:tdls action: peer=%pM, action=%d\n", peer, action);
913
914         switch (action) {
915         case WLAN_TDLS_SETUP_REQUEST:
916                 if (len < (sizeof(struct ethhdr) + TDLS_REQ_FIX_LEN))
917                         return;
918
919                 pos = buf + sizeof(struct ethhdr) + 4;
920                 /* payload 1+ category 1 + action 1 + dialog 1 */
921                 cap = get_unaligned_le16(pos);
922                 ie_len = len - sizeof(struct ethhdr) - TDLS_REQ_FIX_LEN;
923                 pos += 2;
924                 break;
925
926         case WLAN_TDLS_SETUP_RESPONSE:
927                 if (len < (sizeof(struct ethhdr) + TDLS_RESP_FIX_LEN))
928                         return;
929                 /* payload 1+ category 1 + action 1 + dialog 1 + status code 2*/
930                 pos = buf + sizeof(struct ethhdr) + 6;
931                 cap = get_unaligned_le16(pos);
932                 ie_len = len - sizeof(struct ethhdr) - TDLS_RESP_FIX_LEN;
933                 pos += 2;
934                 break;
935
936         case WLAN_TDLS_SETUP_CONFIRM:
937                 if (len < (sizeof(struct ethhdr) + TDLS_CONFIRM_FIX_LEN))
938                         return;
939                 pos = buf + sizeof(struct ethhdr) + TDLS_CONFIRM_FIX_LEN;
940                 ie_len = len - sizeof(struct ethhdr) - TDLS_CONFIRM_FIX_LEN;
941                 break;
942         default:
943                 mwifiex_dbg(priv->adapter, ERROR, "Unknown TDLS frame type.\n");
944                 return;
945         }
946
947         sta_ptr = mwifiex_add_sta_entry(priv, peer);
948         if (!sta_ptr)
949                 return;
950
951         sta_ptr->tdls_cap.capab = cpu_to_le16(cap);
952
953         for (end = pos + ie_len; pos + 1 < end; pos += 2 + pos[1]) {
954                 if (pos + 2 + pos[1] > end)
955                         break;
956
957                 switch (*pos) {
958                 case WLAN_EID_SUPP_RATES:
959                         sta_ptr->tdls_cap.rates_len = pos[1];
960                         for (i = 0; i < pos[1]; i++)
961                                 sta_ptr->tdls_cap.rates[i] = pos[i + 2];
962                         break;
963
964                 case WLAN_EID_EXT_SUPP_RATES:
965                         basic = sta_ptr->tdls_cap.rates_len;
966                         for (i = 0; i < pos[1]; i++)
967                                 sta_ptr->tdls_cap.rates[basic + i] = pos[i + 2];
968                         sta_ptr->tdls_cap.rates_len += pos[1];
969                         break;
970                 case WLAN_EID_HT_CAPABILITY:
971                         memcpy((u8 *)&sta_ptr->tdls_cap.ht_capb, pos,
972                                sizeof(struct ieee80211_ht_cap));
973                         sta_ptr->is_11n_enabled = 1;
974                         break;
975                 case WLAN_EID_HT_OPERATION:
976                         memcpy(&sta_ptr->tdls_cap.ht_oper, pos,
977                                sizeof(struct ieee80211_ht_operation));
978                         break;
979                 case WLAN_EID_BSS_COEX_2040:
980                         sta_ptr->tdls_cap.coex_2040 = pos[2];
981                         break;
982                 case WLAN_EID_EXT_CAPABILITY:
983                         memcpy((u8 *)&sta_ptr->tdls_cap.extcap, pos,
984                                sizeof(struct ieee_types_header) +
985                                min_t(u8, pos[1], 8));
986                         break;
987                 case WLAN_EID_RSN:
988                         memcpy((u8 *)&sta_ptr->tdls_cap.rsn_ie, pos,
989                                sizeof(struct ieee_types_header) +
990                                min_t(u8, pos[1], IEEE_MAX_IE_SIZE -
991                                      sizeof(struct ieee_types_header)));
992                         break;
993                 case WLAN_EID_QOS_CAPA:
994                         sta_ptr->tdls_cap.qos_info = pos[2];
995                         break;
996                 case WLAN_EID_VHT_OPERATION:
997                         if (priv->adapter->is_hw_11ac_capable)
998                                 memcpy(&sta_ptr->tdls_cap.vhtoper, pos,
999                                        sizeof(struct ieee80211_vht_operation));
1000                         break;
1001                 case WLAN_EID_VHT_CAPABILITY:
1002                         if (priv->adapter->is_hw_11ac_capable) {
1003                                 memcpy((u8 *)&sta_ptr->tdls_cap.vhtcap, pos,
1004                                        sizeof(struct ieee80211_vht_cap));
1005                                 sta_ptr->is_11ac_enabled = 1;
1006                         }
1007                         break;
1008                 case WLAN_EID_AID:
1009                         if (priv->adapter->is_hw_11ac_capable)
1010                                 sta_ptr->tdls_cap.aid =
1011                                         get_unaligned_le16((pos + 2));
1012                 default:
1013                         break;
1014                 }
1015         }
1016
1017         return;
1018 }
1019
1020 static int
1021 mwifiex_tdls_process_config_link(struct mwifiex_private *priv, const u8 *peer)
1022 {
1023         struct mwifiex_sta_node *sta_ptr;
1024         struct mwifiex_ds_tdls_oper tdls_oper;
1025
1026         memset(&tdls_oper, 0, sizeof(struct mwifiex_ds_tdls_oper));
1027         sta_ptr = mwifiex_get_sta_entry(priv, peer);
1028
1029         if (!sta_ptr || sta_ptr->tdls_status == TDLS_SETUP_FAILURE) {
1030                 mwifiex_dbg(priv->adapter, ERROR,
1031                             "link absent for peer %pM; cannot config\n", peer);
1032                 return -EINVAL;
1033         }
1034
1035         memcpy(&tdls_oper.peer_mac, peer, ETH_ALEN);
1036         tdls_oper.tdls_action = MWIFIEX_TDLS_CONFIG_LINK;
1037         return mwifiex_send_cmd(priv, HostCmd_CMD_TDLS_OPER,
1038                                 HostCmd_ACT_GEN_SET, 0, &tdls_oper, true);
1039 }
1040
1041 static int
1042 mwifiex_tdls_process_create_link(struct mwifiex_private *priv, const u8 *peer)
1043 {
1044         struct mwifiex_sta_node *sta_ptr;
1045         struct mwifiex_ds_tdls_oper tdls_oper;
1046
1047         memset(&tdls_oper, 0, sizeof(struct mwifiex_ds_tdls_oper));
1048         sta_ptr = mwifiex_get_sta_entry(priv, peer);
1049
1050         if (sta_ptr && sta_ptr->tdls_status == TDLS_SETUP_INPROGRESS) {
1051                 mwifiex_dbg(priv->adapter, WARN,
1052                             "Setup already in progress for peer %pM\n", peer);
1053                 return 0;
1054         }
1055
1056         sta_ptr = mwifiex_add_sta_entry(priv, peer);
1057         if (!sta_ptr)
1058                 return -ENOMEM;
1059
1060         sta_ptr->tdls_status = TDLS_SETUP_INPROGRESS;
1061         mwifiex_hold_tdls_packets(priv, peer);
1062         memcpy(&tdls_oper.peer_mac, peer, ETH_ALEN);
1063         tdls_oper.tdls_action = MWIFIEX_TDLS_CREATE_LINK;
1064         return mwifiex_send_cmd(priv, HostCmd_CMD_TDLS_OPER,
1065                                 HostCmd_ACT_GEN_SET, 0, &tdls_oper, true);
1066 }
1067
1068 static int
1069 mwifiex_tdls_process_disable_link(struct mwifiex_private *priv, const u8 *peer)
1070 {
1071         struct mwifiex_sta_node *sta_ptr;
1072         struct mwifiex_ds_tdls_oper tdls_oper;
1073         unsigned long flags;
1074
1075         memset(&tdls_oper, 0, sizeof(struct mwifiex_ds_tdls_oper));
1076         sta_ptr = mwifiex_get_sta_entry(priv, peer);
1077
1078         if (sta_ptr) {
1079                 if (sta_ptr->is_11n_enabled) {
1080                         mwifiex_11n_cleanup_reorder_tbl(priv);
1081                         spin_lock_irqsave(&priv->wmm.ra_list_spinlock,
1082                                           flags);
1083                         mwifiex_11n_delete_all_tx_ba_stream_tbl(priv);
1084                         spin_unlock_irqrestore(&priv->wmm.ra_list_spinlock,
1085                                                flags);
1086                 }
1087                 mwifiex_del_sta_entry(priv, peer);
1088         }
1089
1090         mwifiex_restore_tdls_packets(priv, peer, TDLS_LINK_TEARDOWN);
1091         mwifiex_auto_tdls_update_peer_status(priv, peer, TDLS_NOT_SETUP);
1092         memcpy(&tdls_oper.peer_mac, peer, ETH_ALEN);
1093         tdls_oper.tdls_action = MWIFIEX_TDLS_DISABLE_LINK;
1094         return mwifiex_send_cmd(priv, HostCmd_CMD_TDLS_OPER,
1095                                 HostCmd_ACT_GEN_SET, 0, &tdls_oper, true);
1096 }
1097
1098 static int
1099 mwifiex_tdls_process_enable_link(struct mwifiex_private *priv, const u8 *peer)
1100 {
1101         struct mwifiex_sta_node *sta_ptr;
1102         struct ieee80211_mcs_info mcs;
1103         unsigned long flags;
1104         int i;
1105
1106         sta_ptr = mwifiex_get_sta_entry(priv, peer);
1107
1108         if (sta_ptr && (sta_ptr->tdls_status != TDLS_SETUP_FAILURE)) {
1109                 mwifiex_dbg(priv->adapter, MSG,
1110                             "tdls: enable link %pM success\n", peer);
1111
1112                 sta_ptr->tdls_status = TDLS_SETUP_COMPLETE;
1113
1114                 mcs = sta_ptr->tdls_cap.ht_capb.mcs;
1115                 if (mcs.rx_mask[0] != 0xff)
1116                         sta_ptr->is_11n_enabled = true;
1117                 if (sta_ptr->is_11n_enabled) {
1118                         if (le16_to_cpu(sta_ptr->tdls_cap.ht_capb.cap_info) &
1119                             IEEE80211_HT_CAP_MAX_AMSDU)
1120                                 sta_ptr->max_amsdu =
1121                                         MWIFIEX_TX_DATA_BUF_SIZE_8K;
1122                         else
1123                                 sta_ptr->max_amsdu =
1124                                         MWIFIEX_TX_DATA_BUF_SIZE_4K;
1125
1126                         for (i = 0; i < MAX_NUM_TID; i++)
1127                                 sta_ptr->ampdu_sta[i] =
1128                                               priv->aggr_prio_tbl[i].ampdu_user;
1129                 } else {
1130                         for (i = 0; i < MAX_NUM_TID; i++)
1131                                 sta_ptr->ampdu_sta[i] = BA_STREAM_NOT_ALLOWED;
1132                 }
1133                 if (sta_ptr->tdls_cap.extcap.ext_capab[3] &
1134                     WLAN_EXT_CAPA4_TDLS_CHAN_SWITCH) {
1135                         mwifiex_config_tdls_enable(priv);
1136                         mwifiex_config_tdls_cs_params(priv);
1137                 }
1138
1139                 memset(sta_ptr->rx_seq, 0xff, sizeof(sta_ptr->rx_seq));
1140                 mwifiex_restore_tdls_packets(priv, peer, TDLS_SETUP_COMPLETE);
1141                 mwifiex_auto_tdls_update_peer_status(priv, peer,
1142                                                      TDLS_SETUP_COMPLETE);
1143         } else {
1144                 mwifiex_dbg(priv->adapter, ERROR,
1145                             "tdls: enable link %pM failed\n", peer);
1146                 if (sta_ptr) {
1147                         mwifiex_11n_cleanup_reorder_tbl(priv);
1148                         spin_lock_irqsave(&priv->wmm.ra_list_spinlock,
1149                                           flags);
1150                         mwifiex_11n_delete_all_tx_ba_stream_tbl(priv);
1151                         spin_unlock_irqrestore(&priv->wmm.ra_list_spinlock,
1152                                                flags);
1153                         mwifiex_del_sta_entry(priv, peer);
1154                 }
1155                 mwifiex_restore_tdls_packets(priv, peer, TDLS_LINK_TEARDOWN);
1156                 mwifiex_auto_tdls_update_peer_status(priv, peer,
1157                                                      TDLS_NOT_SETUP);
1158
1159                 return -1;
1160         }
1161
1162         return 0;
1163 }
1164
1165 int mwifiex_tdls_oper(struct mwifiex_private *priv, const u8 *peer, u8 action)
1166 {
1167         switch (action) {
1168         case MWIFIEX_TDLS_ENABLE_LINK:
1169                 return mwifiex_tdls_process_enable_link(priv, peer);
1170         case MWIFIEX_TDLS_DISABLE_LINK:
1171                 return mwifiex_tdls_process_disable_link(priv, peer);
1172         case MWIFIEX_TDLS_CREATE_LINK:
1173                 return mwifiex_tdls_process_create_link(priv, peer);
1174         case MWIFIEX_TDLS_CONFIG_LINK:
1175                 return mwifiex_tdls_process_config_link(priv, peer);
1176         }
1177         return 0;
1178 }
1179
1180 int mwifiex_get_tdls_link_status(struct mwifiex_private *priv, const u8 *mac)
1181 {
1182         struct mwifiex_sta_node *sta_ptr;
1183
1184         sta_ptr = mwifiex_get_sta_entry(priv, mac);
1185         if (sta_ptr)
1186                 return sta_ptr->tdls_status;
1187
1188         return TDLS_NOT_SETUP;
1189 }
1190
1191 int mwifiex_get_tdls_list(struct mwifiex_private *priv,
1192                           struct tdls_peer_info *buf)
1193 {
1194         struct mwifiex_sta_node *sta_ptr;
1195         struct tdls_peer_info *peer = buf;
1196         int count = 0;
1197         unsigned long flags;
1198
1199         if (!ISSUPP_TDLS_ENABLED(priv->adapter->fw_cap_info))
1200                 return 0;
1201
1202         /* make sure we are in station mode and connected */
1203         if (!(priv->bss_type == MWIFIEX_BSS_TYPE_STA && priv->media_connected))
1204                 return 0;
1205
1206         spin_lock_irqsave(&priv->sta_list_spinlock, flags);
1207         list_for_each_entry(sta_ptr, &priv->sta_list, list) {
1208                 if (mwifiex_is_tdls_link_setup(sta_ptr->tdls_status)) {
1209                         ether_addr_copy(peer->peer_addr, sta_ptr->mac_addr);
1210                         peer++;
1211                         count++;
1212                         if (count >= MWIFIEX_MAX_TDLS_PEER_SUPPORTED)
1213                                 break;
1214                 }
1215         }
1216         spin_unlock_irqrestore(&priv->sta_list_spinlock, flags);
1217
1218         return count;
1219 }
1220
1221 void mwifiex_disable_all_tdls_links(struct mwifiex_private *priv)
1222 {
1223         struct mwifiex_sta_node *sta_ptr;
1224         struct mwifiex_ds_tdls_oper tdls_oper;
1225         unsigned long flags;
1226
1227         if (list_empty(&priv->sta_list))
1228                 return;
1229
1230         list_for_each_entry(sta_ptr, &priv->sta_list, list) {
1231                 memset(&tdls_oper, 0, sizeof(struct mwifiex_ds_tdls_oper));
1232
1233                 if (sta_ptr->is_11n_enabled) {
1234                         mwifiex_11n_cleanup_reorder_tbl(priv);
1235                         spin_lock_irqsave(&priv->wmm.ra_list_spinlock,
1236                                           flags);
1237                         mwifiex_11n_delete_all_tx_ba_stream_tbl(priv);
1238                         spin_unlock_irqrestore(&priv->wmm.ra_list_spinlock,
1239                                                flags);
1240                 }
1241
1242                 mwifiex_restore_tdls_packets(priv, sta_ptr->mac_addr,
1243                                              TDLS_LINK_TEARDOWN);
1244                 memcpy(&tdls_oper.peer_mac, sta_ptr->mac_addr, ETH_ALEN);
1245                 tdls_oper.tdls_action = MWIFIEX_TDLS_DISABLE_LINK;
1246                 if (mwifiex_send_cmd(priv, HostCmd_CMD_TDLS_OPER,
1247                                      HostCmd_ACT_GEN_SET, 0, &tdls_oper, false))
1248                         mwifiex_dbg(priv->adapter, ERROR,
1249                                     "Disable link failed for TDLS peer %pM",
1250                                     sta_ptr->mac_addr);
1251         }
1252
1253         mwifiex_del_all_sta_list(priv);
1254 }
1255
1256 int mwifiex_tdls_check_tx(struct mwifiex_private *priv, struct sk_buff *skb)
1257 {
1258         struct mwifiex_auto_tdls_peer *peer;
1259         unsigned long flags;
1260         u8 mac[ETH_ALEN];
1261
1262         ether_addr_copy(mac, skb->data);
1263
1264         spin_lock_irqsave(&priv->auto_tdls_lock, flags);
1265         list_for_each_entry(peer, &priv->auto_tdls_list, list) {
1266                 if (!memcmp(mac, peer->mac_addr, ETH_ALEN)) {
1267                         if (peer->rssi <= MWIFIEX_TDLS_RSSI_HIGH &&
1268                             peer->tdls_status == TDLS_NOT_SETUP &&
1269                             (peer->failure_count <
1270                              MWIFIEX_TDLS_MAX_FAIL_COUNT)) {
1271                                 peer->tdls_status = TDLS_SETUP_INPROGRESS;
1272                                 mwifiex_dbg(priv->adapter, INFO,
1273                                             "setup TDLS link, peer=%pM rssi=%d\n",
1274                                             peer->mac_addr, peer->rssi);
1275
1276                                 cfg80211_tdls_oper_request(priv->netdev,
1277                                                            peer->mac_addr,
1278                                                            NL80211_TDLS_SETUP,
1279                                                            0, GFP_ATOMIC);
1280                                 peer->do_setup = false;
1281                                 priv->check_tdls_tx = false;
1282                         } else if (peer->failure_count <
1283                                    MWIFIEX_TDLS_MAX_FAIL_COUNT &&
1284                                    peer->do_discover) {
1285                                 mwifiex_send_tdls_data_frame(priv,
1286                                                              peer->mac_addr,
1287                                                     WLAN_TDLS_DISCOVERY_REQUEST,
1288                                                              1, 0, NULL, 0);
1289                                 peer->do_discover = false;
1290                         }
1291                 }
1292         }
1293         spin_unlock_irqrestore(&priv->auto_tdls_lock, flags);
1294
1295         return 0;
1296 }
1297
1298 void mwifiex_flush_auto_tdls_list(struct mwifiex_private *priv)
1299 {
1300         struct mwifiex_auto_tdls_peer *peer, *tmp_node;
1301         unsigned long flags;
1302
1303         spin_lock_irqsave(&priv->auto_tdls_lock, flags);
1304         list_for_each_entry_safe(peer, tmp_node, &priv->auto_tdls_list, list) {
1305                 list_del(&peer->list);
1306                 kfree(peer);
1307         }
1308
1309         INIT_LIST_HEAD(&priv->auto_tdls_list);
1310         spin_unlock_irqrestore(&priv->auto_tdls_lock, flags);
1311         priv->check_tdls_tx = false;
1312 }
1313
1314 void mwifiex_add_auto_tdls_peer(struct mwifiex_private *priv, const u8 *mac)
1315 {
1316         struct mwifiex_auto_tdls_peer *tdls_peer;
1317         unsigned long flags;
1318
1319         if (!priv->adapter->auto_tdls)
1320                 return;
1321
1322         spin_lock_irqsave(&priv->auto_tdls_lock, flags);
1323         list_for_each_entry(tdls_peer, &priv->auto_tdls_list, list) {
1324                 if (!memcmp(tdls_peer->mac_addr, mac, ETH_ALEN)) {
1325                         tdls_peer->tdls_status = TDLS_SETUP_INPROGRESS;
1326                         tdls_peer->rssi_jiffies = jiffies;
1327                         spin_unlock_irqrestore(&priv->auto_tdls_lock, flags);
1328                         return;
1329                 }
1330         }
1331
1332         /* create new TDLS peer */
1333         tdls_peer = kzalloc(sizeof(*tdls_peer), GFP_ATOMIC);
1334         if (tdls_peer) {
1335                 ether_addr_copy(tdls_peer->mac_addr, mac);
1336                 tdls_peer->tdls_status = TDLS_SETUP_INPROGRESS;
1337                 tdls_peer->rssi_jiffies = jiffies;
1338                 INIT_LIST_HEAD(&tdls_peer->list);
1339                 list_add_tail(&tdls_peer->list, &priv->auto_tdls_list);
1340                 mwifiex_dbg(priv->adapter, INFO,
1341                             "Add auto TDLS peer= %pM to list\n", mac);
1342         }
1343
1344         spin_unlock_irqrestore(&priv->auto_tdls_lock, flags);
1345 }
1346
1347 void mwifiex_auto_tdls_update_peer_status(struct mwifiex_private *priv,
1348                                           const u8 *mac, u8 link_status)
1349 {
1350         struct mwifiex_auto_tdls_peer *peer;
1351         unsigned long flags;
1352
1353         if (!priv->adapter->auto_tdls)
1354                 return;
1355
1356         spin_lock_irqsave(&priv->auto_tdls_lock, flags);
1357         list_for_each_entry(peer, &priv->auto_tdls_list, list) {
1358                 if (!memcmp(peer->mac_addr, mac, ETH_ALEN)) {
1359                         if ((link_status == TDLS_NOT_SETUP) &&
1360                             (peer->tdls_status == TDLS_SETUP_INPROGRESS))
1361                                 peer->failure_count++;
1362                         else if (mwifiex_is_tdls_link_setup(link_status))
1363                                 peer->failure_count = 0;
1364
1365                         peer->tdls_status = link_status;
1366                         break;
1367                 }
1368         }
1369         spin_unlock_irqrestore(&priv->auto_tdls_lock, flags);
1370 }
1371
1372 void mwifiex_auto_tdls_update_peer_signal(struct mwifiex_private *priv,
1373                                           u8 *mac, s8 snr, s8 nflr)
1374 {
1375         struct mwifiex_auto_tdls_peer *peer;
1376         unsigned long flags;
1377
1378         if (!priv->adapter->auto_tdls)
1379                 return;
1380
1381         spin_lock_irqsave(&priv->auto_tdls_lock, flags);
1382         list_for_each_entry(peer, &priv->auto_tdls_list, list) {
1383                 if (!memcmp(peer->mac_addr, mac, ETH_ALEN)) {
1384                         peer->rssi = nflr - snr;
1385                         peer->rssi_jiffies = jiffies;
1386                         break;
1387                 }
1388         }
1389         spin_unlock_irqrestore(&priv->auto_tdls_lock, flags);
1390 }
1391
1392 void mwifiex_check_auto_tdls(unsigned long context)
1393 {
1394         struct mwifiex_private *priv = (struct mwifiex_private *)context;
1395         struct mwifiex_auto_tdls_peer *tdls_peer;
1396         unsigned long flags;
1397         u16 reason = WLAN_REASON_TDLS_TEARDOWN_UNSPECIFIED;
1398
1399         if (WARN_ON_ONCE(!priv || !priv->adapter)) {
1400                 pr_err("mwifiex: %s: adapter or private structure is NULL\n",
1401                        __func__);
1402                 return;
1403         }
1404
1405         if (unlikely(!priv->adapter->auto_tdls))
1406                 return;
1407
1408         if (!priv->auto_tdls_timer_active) {
1409                 mwifiex_dbg(priv->adapter, INFO,
1410                             "auto TDLS timer inactive; return");
1411                 return;
1412         }
1413
1414         priv->check_tdls_tx = false;
1415
1416         if (list_empty(&priv->auto_tdls_list)) {
1417                 mod_timer(&priv->auto_tdls_timer,
1418                           jiffies +
1419                           msecs_to_jiffies(MWIFIEX_TIMER_10S));
1420                 return;
1421         }
1422
1423         spin_lock_irqsave(&priv->auto_tdls_lock, flags);
1424         list_for_each_entry(tdls_peer, &priv->auto_tdls_list, list) {
1425                 if ((jiffies - tdls_peer->rssi_jiffies) >
1426                     (MWIFIEX_AUTO_TDLS_IDLE_TIME * HZ)) {
1427                         tdls_peer->rssi = 0;
1428                         tdls_peer->do_discover = true;
1429                         priv->check_tdls_tx = true;
1430                 }
1431
1432                 if (((tdls_peer->rssi >= MWIFIEX_TDLS_RSSI_LOW) ||
1433                      !tdls_peer->rssi) &&
1434                     mwifiex_is_tdls_link_setup(tdls_peer->tdls_status)) {
1435                         tdls_peer->tdls_status = TDLS_LINK_TEARDOWN;
1436                         mwifiex_dbg(priv->adapter, MSG,
1437                                     "teardown TDLS link,peer=%pM rssi=%d\n",
1438                                     tdls_peer->mac_addr, -tdls_peer->rssi);
1439                         tdls_peer->do_discover = true;
1440                         priv->check_tdls_tx = true;
1441                         cfg80211_tdls_oper_request(priv->netdev,
1442                                                    tdls_peer->mac_addr,
1443                                                    NL80211_TDLS_TEARDOWN,
1444                                                    reason, GFP_ATOMIC);
1445                 } else if (tdls_peer->rssi &&
1446                            tdls_peer->rssi <= MWIFIEX_TDLS_RSSI_HIGH &&
1447                            tdls_peer->tdls_status == TDLS_NOT_SETUP &&
1448                            tdls_peer->failure_count <
1449                            MWIFIEX_TDLS_MAX_FAIL_COUNT) {
1450                                 priv->check_tdls_tx = true;
1451                                 tdls_peer->do_setup = true;
1452                                 mwifiex_dbg(priv->adapter, INFO,
1453                                             "check TDLS with peer=%pM\t"
1454                                             "rssi=%d\n", tdls_peer->mac_addr,
1455                                             tdls_peer->rssi);
1456                 }
1457         }
1458         spin_unlock_irqrestore(&priv->auto_tdls_lock, flags);
1459
1460         mod_timer(&priv->auto_tdls_timer,
1461                   jiffies + msecs_to_jiffies(MWIFIEX_TIMER_10S));
1462 }
1463
1464 void mwifiex_setup_auto_tdls_timer(struct mwifiex_private *priv)
1465 {
1466         setup_timer(&priv->auto_tdls_timer, mwifiex_check_auto_tdls,
1467                     (unsigned long)priv);
1468         priv->auto_tdls_timer_active = true;
1469         mod_timer(&priv->auto_tdls_timer,
1470                   jiffies + msecs_to_jiffies(MWIFIEX_TIMER_10S));
1471 }
1472
1473 void mwifiex_clean_auto_tdls(struct mwifiex_private *priv)
1474 {
1475         if (ISSUPP_TDLS_ENABLED(priv->adapter->fw_cap_info) &&
1476             priv->adapter->auto_tdls &&
1477             priv->bss_type == MWIFIEX_BSS_TYPE_STA) {
1478                 priv->auto_tdls_timer_active = false;
1479                 del_timer(&priv->auto_tdls_timer);
1480                 mwifiex_flush_auto_tdls_list(priv);
1481         }
1482 }
1483
1484 static int mwifiex_config_tdls(struct mwifiex_private *priv, u8 enable)
1485 {
1486         struct mwifiex_tdls_config config;
1487
1488         config.enable = cpu_to_le16(enable);
1489         return mwifiex_send_cmd(priv, HostCmd_CMD_TDLS_CONFIG,
1490                                 ACT_TDLS_CS_ENABLE_CONFIG, 0, &config, true);
1491 }
1492
1493 int mwifiex_config_tdls_enable(struct mwifiex_private *priv)
1494 {
1495         return mwifiex_config_tdls(priv, true);
1496 }
1497
1498 int mwifiex_config_tdls_disable(struct mwifiex_private *priv)
1499 {
1500         return mwifiex_config_tdls(priv, false);
1501 }
1502
1503 int mwifiex_config_tdls_cs_params(struct mwifiex_private *priv)
1504 {
1505         struct mwifiex_tdls_config_cs_params config_tdls_cs_params;
1506
1507         config_tdls_cs_params.unit_time = MWIFIEX_DEF_CS_UNIT_TIME;
1508         config_tdls_cs_params.thr_otherlink = MWIFIEX_DEF_CS_THR_OTHERLINK;
1509         config_tdls_cs_params.thr_directlink = MWIFIEX_DEF_THR_DIRECTLINK;
1510
1511         return mwifiex_send_cmd(priv, HostCmd_CMD_TDLS_CONFIG,
1512                                 ACT_TDLS_CS_PARAMS, 0,
1513                                 &config_tdls_cs_params, true);
1514 }
1515
1516 int mwifiex_stop_tdls_cs(struct mwifiex_private *priv, const u8 *peer_mac)
1517 {
1518         struct mwifiex_tdls_stop_cs_params stop_tdls_cs_params;
1519
1520         ether_addr_copy(stop_tdls_cs_params.peer_mac, peer_mac);
1521
1522         return mwifiex_send_cmd(priv, HostCmd_CMD_TDLS_CONFIG,
1523                                 ACT_TDLS_CS_STOP, 0,
1524                                 &stop_tdls_cs_params, true);
1525 }
1526
1527 int mwifiex_start_tdls_cs(struct mwifiex_private *priv, const u8 *peer_mac,
1528                           u8 primary_chan, u8 second_chan_offset, u8 band)
1529 {
1530         struct mwifiex_tdls_init_cs_params start_tdls_cs_params;
1531
1532         ether_addr_copy(start_tdls_cs_params.peer_mac, peer_mac);
1533         start_tdls_cs_params.primary_chan = primary_chan;
1534         start_tdls_cs_params.second_chan_offset = second_chan_offset;
1535         start_tdls_cs_params.band = band;
1536
1537         start_tdls_cs_params.switch_time = cpu_to_le16(MWIFIEX_DEF_CS_TIME);
1538         start_tdls_cs_params.switch_timeout =
1539                                         cpu_to_le16(MWIFIEX_DEF_CS_TIMEOUT);
1540         start_tdls_cs_params.reg_class = MWIFIEX_DEF_CS_REG_CLASS;
1541         start_tdls_cs_params.periodicity = MWIFIEX_DEF_CS_PERIODICITY;
1542
1543         return mwifiex_send_cmd(priv, HostCmd_CMD_TDLS_CONFIG,
1544                                 ACT_TDLS_CS_INIT, 0,
1545                                 &start_tdls_cs_params, true);
1546 }