drivers/net/wireless/rockchip_wlan/rtl8723cs/core/rtw_beamforming.c

   1 /******************************************************************************
   2  *
   3  * Copyright(c) 2007 - 2016 Realtek Corporation. All rights reserved.
   4  *
   5  * This program is free software; you can redistribute it and/or modify it
   6  * under the terms of version 2 of the GNU General Public License as
   7  * published by the Free Software Foundation.
   8  *
   9  * This program is distributed in the hope that it will be useful, but WITHOUT
  10  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
  11  * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
  12  * more details.
  13  *
  14  * You should have received a copy of the GNU General Public License along with
  15  * this program; if not, write to the Free Software Foundation, Inc.,
  16  * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
  17  *
  18  *
  19  ******************************************************************************/
  20 #define _RTW_BEAMFORMING_C_
  21
  22 #include <drv_types.h>
  23 #include <hal_data.h>
  24
  25 #ifdef CONFIG_BEAMFORMING
  26
  27 #ifdef RTW_BEAMFORMING_VERSION_2
  28
  29 struct ndpa_sta_info {
  30         u16 aid:12;
  31         u16 feedback_type:1;
  32         u16 nc_index:3;
  33 };
  34
  35 static void _get_txvector_parameter(PADAPTER adapter, struct sta_info *sta, u8 *g_id, u16 *p_aid)
  36 {
  37         struct mlme_priv *mlme;
  38         u16 aid;
  39         u8 *bssid;
  40         u16 val16;
  41         u8 i;
  42
  43
  44         mlme = &adapter->mlmepriv;
  45
  46         if (check_fwstate(mlme, WIFI_AP_STATE)) {
  47                 /*
  48                  * Sent by an AP and addressed to a STA associated with that AP
  49                  * or sent by a DLS or TDLS STA in a direct path to
  50                  * a DLS or TDLS peer STA
  51                  */
  52
  53                 aid = sta->aid;
  54                 bssid = adapter_mac_addr(adapter);
  55                 RTW_INFO("%s: AID=0x%x BSSID=" MAC_FMT "\n",
  56                          __FUNCTION__, sta->aid, MAC_ARG(bssid));
  57
  58                 /* AID[0:8] */
  59                 aid &= 0x1FF;
  60                 /* BSSID[44:47] xor BSSID[40:43] */
  61                 val16 = ((bssid[5] & 0xF0) >> 4) ^ (bssid[5] & 0xF);
  62                 /* (dec(AID[0:8]) + dec(BSSID)*2^5) mod 2^9 */
  63                 *p_aid = (aid + (val16 << 5)) & 0x1FF;
  64                 *g_id = 63;
  65         } else if ((check_fwstate(mlme, WIFI_ADHOC_STATE) == _TRUE)
  66                    || (check_fwstate(mlme, WIFI_ADHOC_MASTER_STATE) == _TRUE)) {
  67                 /*
  68                  * Otherwise, includes
  69                  * 1. Sent to an IBSS STA
  70                  * 2. Sent by an AP to a non associated STA
  71                  * 3. Sent to a STA for which it is not known
  72                  *    which condition is applicable
  73                  */
  74                 *p_aid = 0;
  75                 *g_id = 63;
  76         } else {
  77                 /* Addressed to AP */
  78                 bssid = sta->hwaddr;
  79                 RTW_INFO("%s: BSSID=" MAC_FMT "\n", __FUNCTION__, MAC_ARG(bssid));
  80
  81                 /* BSSID[39:47] */
  82                 *p_aid = (bssid[5] << 1) | (bssid[4] >> 7);
  83                 *g_id = 0;
  84         }
  85
  86         RTW_INFO("%s: GROUP_ID=0x%02x PARTIAL_AID=0x%04x\n",
  87                  __FUNCTION__, *g_id, *p_aid);
  88 }
  89
  90 /*
  91  * Parameters
  92  *      adapter         struct _adapter*
  93  *      sta             struct sta_info*
  94  *      sta_bf_cap      beamforming capabe of sta
  95  *      sounding_dim    Number of Sounding Dimensions
  96  *      comp_steering   Compressed Steering Number of Beamformer Antennas Supported
  97  */
  98 static void _get_sta_beamform_cap(PADAPTER adapter, struct sta_info *sta,
  99         u8 *sta_bf_cap, u8 *sounding_dim, u8 *comp_steering)
 100 {
 101         struct beamforming_info *info;
 102         struct ht_priv *ht;
 103 #ifdef CONFIG_80211AC_VHT
 104         struct vht_priv *vht;
 105 #endif /* CONFIG_80211AC_VHT */
 106         u16 bf_cap;
 107
 108
 109         *sta_bf_cap = 0;
 110         *sounding_dim = 0;
 111         *comp_steering = 0;
 112
 113         info = GET_BEAMFORM_INFO(adapter);
 114         ht = &adapter->mlmepriv.htpriv;
 115 #ifdef CONFIG_80211AC_VHT
 116         vht = &adapter->mlmepriv.vhtpriv;
 117 #endif /* CONFIG_80211AC_VHT */
 118
 119         if (is_supported_ht(sta->wireless_mode) == _TRUE) {
 120                 /* HT */
 121                 bf_cap = ht->beamform_cap;
 122
 123                 if (TEST_FLAG(bf_cap, BEAMFORMING_HT_BEAMFORMEE_ENABLE)) {
 124                         info->beamforming_cap |= BEAMFORMEE_CAP_HT_EXPLICIT;
 125                         *sta_bf_cap |= BEAMFORMER_CAP_HT_EXPLICIT;
 126                         *sounding_dim = (bf_cap & BEAMFORMING_HT_BEAMFORMEE_CHNL_EST_CAP) >> 6;
 127                 }
 128                 if (TEST_FLAG(bf_cap, BEAMFORMING_HT_BEAMFORMER_ENABLE)) {
 129                         info->beamforming_cap |= BEAMFORMER_CAP_HT_EXPLICIT;
 130                         *sta_bf_cap |= BEAMFORMEE_CAP_HT_EXPLICIT;
 131                         *comp_steering = (bf_cap & BEAMFORMING_HT_BEAMFORMER_STEER_NUM) >> 4;
 132                 }
 133         }
 134
 135 #ifdef CONFIG_80211AC_VHT
 136         if (is_supported_vht(sta->wireless_mode) == _TRUE) {
 137                 /* VHT */
 138                 bf_cap = vht->beamform_cap;
 139
 140                 /* We are SU Beamformee because the STA is SU Beamformer */
 141                 if (TEST_FLAG(bf_cap, BEAMFORMING_VHT_BEAMFORMEE_ENABLE)) {
 142                         info->beamforming_cap |= BEAMFORMEE_CAP_VHT_SU;
 143                         *sta_bf_cap |= BEAMFORMER_CAP_VHT_SU;
 144
 145                         /* We are MU Beamformee because the STA is MU Beamformer */
 146                         if (TEST_FLAG(bf_cap, BEAMFORMING_VHT_MU_MIMO_STA_ENABLE)) {
 147                                 info->beamforming_cap |= BEAMFORMEE_CAP_VHT_MU;
 148                                 *sta_bf_cap |= BEAMFORMER_CAP_VHT_MU;
 149                         }
 150
 151                         *sounding_dim = (bf_cap & BEAMFORMING_VHT_BEAMFORMEE_SOUND_DIM) >> 12;
 152                 }
 153                 /* We are SU Beamformer because the STA is SU Beamformee */
 154                 if (TEST_FLAG(bf_cap, BEAMFORMING_VHT_BEAMFORMER_ENABLE)) {
 155                         info->beamforming_cap |= BEAMFORMER_CAP_VHT_SU;
 156                         *sta_bf_cap |= BEAMFORMEE_CAP_VHT_SU;
 157
 158                         /* We are MU Beamformer because the STA is MU Beamformee */
 159                         if (TEST_FLAG(bf_cap, BEAMFORMING_VHT_MU_MIMO_AP_ENABLE)) {
 160                                 info->beamforming_cap |= BEAMFORMER_CAP_VHT_MU;
 161                                 *sta_bf_cap |= BEAMFORMEE_CAP_VHT_MU;
 162                         }
 163
 164                         *comp_steering = (bf_cap & BEAMFORMING_VHT_BEAMFORMER_STS_CAP) >> 8;
 165                 }
 166         }
 167 #endif /* CONFIG_80211AC_VHT */
 168 }
 169
 170 static u8 _send_ht_ndpa_packet(PADAPTER adapter, u8 *ra, CHANNEL_WIDTH bw)
 171 {
 172         /* General */
 173         struct xmit_priv                *pxmitpriv;
 174         struct mlme_ext_priv            *pmlmeext;
 175         struct mlme_ext_info            *pmlmeinfo;
 176         struct xmit_frame               *pmgntframe;
 177         /* Beamforming */
 178         struct beamforming_info         *info;
 179         struct beamformee_entry         *bfee;
 180         struct ndpa_sta_info            sta_info;
 181         u8 ActionHdr[4] = {ACT_CAT_VENDOR, 0x00, 0xE0, 0x4C};
 182         /* MISC */
 183         struct pkt_attrib               *attrib;
 184         struct rtw_ieee80211_hdr        *pwlanhdr;
 185         enum MGN_RATE txrate;
 186         u8 *pframe;
 187         u16 duration = 0;
 188         u8 aSifsTime = 0;
 189
 190
 191         RTW_INFO("+%s: Send to " MAC_FMT "\n", __FUNCTION__, MAC_ARG(ra));
 192
 193         pxmitpriv = &adapter->xmitpriv;
 194         pmlmeext = &adapter->mlmeextpriv;
 195         pmlmeinfo = &pmlmeext->mlmext_info;
 196         bfee = rtw_bf_bfee_get_entry_by_addr(adapter, ra);
 197         if (!bfee) {
 198                 RTW_ERR("%s: Cann't find beamformee entry!\n", __FUNCTION__);
 199                 return _FALSE;
 200         }
 201
 202         pmgntframe = alloc_mgtxmitframe(pxmitpriv);
 203         if (!pmgntframe) {
 204                 RTW_ERR("%s: alloc mgnt frame fail!\n", __FUNCTION__);
 205                 return _FALSE;
 206         }
 207
 208         txrate = beamforming_get_htndp_tx_rate(GET_PDM_ODM(adapter), bfee->comp_steering_num_of_bfer);
 209
 210         /* update attribute */
 211         attrib = &pmgntframe->attrib;
 212         update_mgntframe_attrib(adapter, attrib);
 213         /*attrib->type = WIFI_MGT_TYPE;*/ /* set in update_mgntframe_attrib() */
 214         attrib->subtype = WIFI_ACTION_NOACK;
 215         attrib->bwmode = bw;
 216         /*attrib->qsel = QSLT_MGNT;*/ /* set in update_mgntframe_attrib() */
 217         attrib->order = 1;
 218         attrib->rate = (u8)txrate;
 219         attrib->bf_pkt_type = 0;
 220
 221         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
 222         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
 223         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
 224
 225         /* Frame control */
 226         pwlanhdr->frame_ctl = 0;
 227         set_frame_sub_type(pframe, attrib->subtype);
 228         set_order_bit(pframe);
 229
 230         /* Duration */
 231         if (pmlmeext->cur_wireless_mode == WIRELESS_11B)
 232                 aSifsTime = 10;
 233         else
 234                 aSifsTime = 16;
 235         duration = 2 * aSifsTime + 40;
 236         if (bw == CHANNEL_WIDTH_40)
 237                 duration += 87;
 238         else
 239                 duration += 180;
 240         set_duration(pframe, duration);
 241
 242         /* DA */
 243         _rtw_memcpy(pwlanhdr->addr1, ra, ETH_ALEN);
 244         /* SA */
 245         _rtw_memcpy(pwlanhdr->addr2, adapter_mac_addr(adapter), ETH_ALEN);
 246         /* BSSID */
 247         _rtw_memcpy(pwlanhdr->addr3, get_my_bssid(&pmlmeinfo->network), ETH_ALEN);
 248
 249         /* HT control field */
 250         SET_HT_CTRL_CSI_STEERING(pframe + 24, 3);
 251         SET_HT_CTRL_NDP_ANNOUNCEMENT(pframe + 24, 1);
 252
 253         /*
 254          * Frame Body
 255          * Category field: vender-specific value, 0x7F
 256          * OUI: 0x00E04C
 257          */
 258         _rtw_memcpy(pframe + 28, ActionHdr, 4);
 259
 260         attrib->pktlen = 32;
 261         attrib->last_txcmdsz = attrib->pktlen;
 262
 263         dump_mgntframe(adapter, pmgntframe);
 264
 265         return _TRUE;
 266 }
 267
 268 static u8 _send_vht_ndpa_packet(PADAPTER adapter, u8 *ra, u16 aid, CHANNEL_WIDTH bw)
 269 {
 270         /* General */
 271         struct xmit_priv                *pxmitpriv;
 272         struct mlme_ext_priv            *pmlmeext;
 273         struct xmit_frame               *pmgntframe;
 274         /* Beamforming */
 275         struct beamforming_info         *info;
 276         struct beamformee_entry         *bfee;
 277         struct ndpa_sta_info            sta_info;
 278         /* MISC */
 279         struct pkt_attrib               *attrib;
 280         struct rtw_ieee80211_hdr        *pwlanhdr;
 281         u8 *pframe;
 282         enum MGN_RATE txrate;
 283         u16 duration = 0;
 284         u8 sequence = 0, aSifsTime = 0;
 285
 286
 287         RTW_INFO("+%s: Send to " MAC_FMT "\n", __FUNCTION__, MAC_ARG(ra));
 288
 289         pxmitpriv = &adapter->xmitpriv;
 290         pmlmeext = &adapter->mlmeextpriv;
 291         info = GET_BEAMFORM_INFO(adapter);
 292         bfee = rtw_bf_bfee_get_entry_by_addr(adapter, ra);
 293         if (!bfee) {
 294                 RTW_ERR("%s: Cann't find beamformee entry!\n", __FUNCTION__);
 295                 return _FALSE;
 296         }
 297
 298         pmgntframe = alloc_mgtxmitframe(pxmitpriv);
 299         if (!pmgntframe) {
 300                 RTW_ERR("%s: alloc mgnt frame fail!\n", __FUNCTION__);
 301                 return _FALSE;
 302         }
 303
 304         txrate = beamforming_get_vht_ndp_tx_rate(GET_PDM_ODM(adapter), bfee->comp_steering_num_of_bfer);
 305
 306         /* update attribute */
 307         attrib = &pmgntframe->attrib;
 308         update_mgntframe_attrib(adapter, attrib);
 309         /*pattrib->type = WIFI_MGT_TYPE;*/ /* set in update_mgntframe_attrib() */
 310         attrib->subtype = WIFI_NDPA;
 311         attrib->bwmode = bw;
 312         /*attrib->qsel = QSLT_MGNT;*/ /* set in update_mgntframe_attrib() */
 313         attrib->rate = (u8)txrate;
 314         attrib->bf_pkt_type = 0;
 315
 316         _rtw_memset(pmgntframe->buf_addr, 0, TXDESC_OFFSET + WLANHDR_OFFSET);
 317         pframe = pmgntframe->buf_addr + TXDESC_OFFSET;
 318         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
 319
 320         /* Frame control */
 321         pwlanhdr->frame_ctl = 0;
 322         set_frame_sub_type(pframe, attrib->subtype);
 323
 324         /* Duration */
 325         if (is_supported_5g(pmlmeext->cur_wireless_mode) || is_supported_ht(pmlmeext->cur_wireless_mode))
 326                 aSifsTime = 16;
 327         else
 328                 aSifsTime = 10;
 329         duration = 2 * aSifsTime + 44;
 330         if (bw == CHANNEL_WIDTH_80)
 331                 duration += 40;
 332         else if (bw == CHANNEL_WIDTH_40)
 333                 duration += 87;
 334         else
 335                 duration += 180;
 336         set_duration(pframe, duration);
 337
 338         /* RA */
 339         _rtw_memcpy(pwlanhdr->addr1, ra, ETH_ALEN);
 340
 341         /* TA */
 342         _rtw_memcpy(pwlanhdr->addr2, adapter_mac_addr(adapter), ETH_ALEN);
 343
 344         /* Sounding Sequence, bit0~1 is reserved */
 345         sequence = info->sounding_sequence << 2;
 346         if (info->sounding_sequence >= 0x3f)
 347                 info->sounding_sequence = 0;
 348         else
 349                 info->sounding_sequence++;
 350         _rtw_memcpy(pframe + 16, &sequence, 1);
 351
 352         /* STA Info */
 353         /*
 354          * "AID12" Equal to 0 if the STA is an AP, mesh STA or
 355          * STA that is a member of an IBSS
 356          */
 357         if (check_fwstate(&adapter->mlmepriv, WIFI_AP_STATE) == _FALSE)
 358                 aid = 0;
 359         sta_info.aid = aid;
 360         /* "Feedback Type" set to 0 for SU */
 361         sta_info.feedback_type = 0;
 362         /* "Nc Index" reserved if the Feedback Type field indicates SU */
 363         sta_info.nc_index = 0;
 364         _rtw_memcpy(pframe + 17, (u8 *)&sta_info, 2);
 365
 366         attrib->pktlen = 19;
 367         attrib->last_txcmdsz = attrib->pktlen;
 368
 369         dump_mgntframe(adapter, pmgntframe);
 370
 371         return _TRUE;
 372 }
 373
 374 static u8 _send_vht_mu_ndpa_packet(PADAPTER adapter, CHANNEL_WIDTH bw)
 375 {
 376         /* General */
 377         struct xmit_priv                *pxmitpriv;
 378         struct mlme_ext_priv            *pmlmeext;
 379         struct xmit_frame               *pmgntframe;
 380         /* Beamforming */
 381         struct beamforming_info         *info;
 382         struct sounding_info            *sounding;
 383         struct beamformee_entry         *bfee;
 384         struct ndpa_sta_info            sta_info;
 385         /* MISC */
 386         struct pkt_attrib               *attrib;
 387         struct rtw_ieee80211_hdr        *pwlanhdr;
 388         enum MGN_RATE txrate;
 389         u8 *pframe;
 390         u8 *ra = NULL;
 391         u16 duration = 0;
 392         u8 sequence = 0, aSifsTime = 0;
 393         u8 i;
 394
 395
 396         RTW_INFO("+%s\n", __FUNCTION__);
 397
 398         pxmitpriv = &adapter->xmitpriv;
 399         pmlmeext = &adapter->mlmeextpriv;
 400         info = GET_BEAMFORM_INFO(adapter);
 401         sounding = &info->sounding_info;
 402
 403         txrate = MGN_VHT2SS_MCS0;
 404
 405         /*
 406          * Fill the first MU BFee entry (STA1) MAC addr to destination address then
 407          * HW will change A1 to broadcast addr.
 408          * 2015.05.28. Suggested by SD1 Chunchu.
 409          */
 410         bfee = &info->bfee_entry[sounding->mu_sounding_list[0]];
 411         ra = bfee->mac_addr;
 412
 413         pmgntframe = alloc_mgtxmitframe(pxmitpriv);
 414         if (!pmgntframe) {
 415                 RTW_ERR("%s: alloc mgnt frame fail!\n", __FUNCTION__);
 416                 return _FALSE;
 417         }
 418
 419         /* update attribute */
 420         attrib = &pmgntframe->attrib;
 421         update_mgntframe_attrib(adapter, attrib);
 422         /*attrib->type = WIFI_MGT_TYPE;*/ /* set in update_mgntframe_attrib() */
 423         attrib->subtype = WIFI_NDPA;
 424         attrib->bwmode = bw;
 425         /*attrib->qsel = QSLT_MGNT;*/ /* set in update_mgntframe_attrib() */
 426         attrib->rate = (u8)txrate;
 427         /* Set TxBFPktType of Tx desc to unicast type if there is only one MU STA for HW design */
 428         if (info->sounding_info.candidate_mu_bfee_cnt > 1)
 429                 attrib->bf_pkt_type = 1;
 430         else
 431                 attrib->bf_pkt_type = 0;
 432
 433         _rtw_memset(pmgntframe->buf_addr, 0, TXDESC_OFFSET + WLANHDR_OFFSET);
 434         pframe = pmgntframe->buf_addr + TXDESC_OFFSET;
 435         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
 436
 437         /* Frame control */
 438         pwlanhdr->frame_ctl = 0;
 439         set_frame_sub_type(pframe, attrib->subtype);
 440
 441         /* Duration */
 442         if (is_supported_5g(pmlmeext->cur_wireless_mode) || is_supported_ht(pmlmeext->cur_wireless_mode))
 443                 aSifsTime = 16;
 444         else
 445                 aSifsTime = 10;
 446         duration = 2 * aSifsTime + 44;
 447         if (bw == CHANNEL_WIDTH_80)
 448                 duration += 40;
 449         else if (bw == CHANNEL_WIDTH_40)
 450                 duration += 87;
 451         else
 452                 duration += 180;
 453         set_duration(pframe, duration);
 454
 455         /* RA */
 456         _rtw_memcpy(pwlanhdr->addr1, ra, ETH_ALEN);
 457
 458         /* TA */
 459         _rtw_memcpy(pwlanhdr->addr2, adapter_mac_addr(adapter), ETH_ALEN);
 460
 461         /* Sounding Sequence, bit0~1 is reserved */
 462         sequence = info->sounding_sequence << 2;
 463         if (info->sounding_sequence >= 0x3f)
 464                 info->sounding_sequence = 0;
 465         else
 466                 info->sounding_sequence++;
 467         _rtw_memcpy(pframe + 16, &sequence, 1);
 468
 469         attrib->pktlen = 17;
 470
 471         /*
 472          * Construct STA info. for multiple STAs
 473          * STA Info1, ..., STA Info n
 474          */
 475         for (i = 0; i < sounding->candidate_mu_bfee_cnt; i++) {
 476                 bfee = &info->bfee_entry[sounding->mu_sounding_list[i]];
 477                 sta_info.aid = bfee->aid;
 478                 sta_info.feedback_type = 1; /* 1'b1: MU */
 479                 sta_info.nc_index = 0;
 480                 _rtw_memcpy(pframe + attrib->pktlen, (u8 *)&sta_info, 2);
 481                 attrib->pktlen += 2;
 482         }
 483
 484         attrib->last_txcmdsz = attrib->pktlen;
 485
 486         dump_mgntframe(adapter, pmgntframe);
 487
 488         return _TRUE;
 489 }
 490
 491 static u8 _send_bf_report_poll(PADAPTER adapter, u8 *ra, u8 bFinalPoll)
 492 {
 493         /* General */
 494         struct xmit_priv *pxmitpriv;
 495         struct xmit_frame *pmgntframe;
 496         /* MISC */
 497         struct pkt_attrib *attrib;
 498         struct rtw_ieee80211_hdr *pwlanhdr;
 499         u8 *pframe;
 500
 501
 502         RTW_INFO("+%s: Send to " MAC_FMT "\n", __FUNCTION__, MAC_ARG(ra));
 503
 504         pxmitpriv = &adapter->xmitpriv;
 505
 506         pmgntframe = alloc_mgtxmitframe(pxmitpriv);
 507         if (!pmgntframe) {
 508                 RTW_ERR("%s: alloc mgnt frame fail!\n", __FUNCTION__);
 509                 return _FALSE;
 510         }
 511
 512         /* update attribute */
 513         attrib = &pmgntframe->attrib;
 514         update_mgntframe_attrib(adapter, attrib);
 515         /*attrib->type = WIFI_MGT_TYPE;*/ /* set in update_mgntframe_attrib() */
 516         attrib->subtype = WIFI_BF_REPORT_POLL;
 517         attrib->bwmode = CHANNEL_WIDTH_20;
 518         /*attrib->qsel = QSLT_MGNT;*/ /* set in update_mgntframe_attrib() */
 519         attrib->rate = MGN_6M;
 520         if (bFinalPoll)
 521                 attrib->bf_pkt_type = 3;
 522         else
 523                 attrib->bf_pkt_type = 2;
 524
 525         _rtw_memset(pmgntframe->buf_addr, 0, TXDESC_OFFSET + WLANHDR_OFFSET);
 526         pframe = pmgntframe->buf_addr + TXDESC_OFFSET;
 527         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
 528
 529         /* Frame control */
 530         pwlanhdr->frame_ctl = 0;
 531         set_frame_sub_type(pframe, attrib->subtype);
 532
 533         /* Duration */
 534         set_duration(pframe, 100);
 535
 536         /* RA */
 537         _rtw_memcpy(pwlanhdr->addr1, ra, ETH_ALEN);
 538
 539         /* TA */
 540         _rtw_memcpy(pwlanhdr->addr2, adapter_mac_addr(adapter), ETH_ALEN);
 541
 542         /* Feedback Segment Retransmission Bitmap */
 543         pframe[16] = 0xFF;
 544
 545         attrib->pktlen = 17;
 546         attrib->last_txcmdsz = attrib->pktlen;
 547
 548         dump_mgntframe(adapter, pmgntframe);
 549
 550         return _TRUE;
 551 }
 552
 553 static void _sounding_update_min_period(PADAPTER adapter, u16 period, u8 leave)
 554 {
 555         struct beamforming_info *info;
 556         struct beamformee_entry *bfee;
 557         u8 i = 0;
 558         u16 min_val = 0xFFFF;
 559
 560
 561         info = GET_BEAMFORM_INFO(adapter);
 562
 563         if (_TRUE == leave) {
 564                 /*
 565                  * When a BFee left,
 566                  * we need to find the latest min sounding period
 567                  * from the remaining BFees
 568                  */
 569                 for (i = 0; i < MAX_BEAMFORMEE_ENTRY_NUM; i++) {
 570                         bfee = &info->bfee_entry[i];
 571                         if ((bfee->used == _TRUE)
 572                             && (bfee->sound_period < min_val))
 573                                 min_val = bfee->sound_period;
 574                 }
 575
 576                 if (min_val == 0xFFFF)
 577                         info->sounding_info.min_sounding_period = 0;
 578                 else
 579                         info->sounding_info.min_sounding_period = min_val;
 580         } else {
 581                 if ((info->sounding_info.min_sounding_period == 0)
 582                     || (period < info->sounding_info.min_sounding_period))
 583                         info->sounding_info.min_sounding_period = period;
 584         }
 585 }
 586
 587 static void _sounding_init(struct sounding_info *sounding)
 588 {
 589         _rtw_memset(sounding->su_sounding_list, 0xFF, MAX_NUM_BEAMFORMEE_SU);
 590         _rtw_memset(sounding->mu_sounding_list, 0xFF, MAX_NUM_BEAMFORMEE_MU);
 591         sounding->state = SOUNDING_STATE_NONE;
 592         sounding->su_bfee_curidx = 0xFF;
 593         sounding->candidate_mu_bfee_cnt = 0;
 594         sounding->min_sounding_period = 0;
 595         sounding->sound_remain_cnt_per_period = 0;
 596 }
 597
 598 static void _sounding_reset_vars(PADAPTER adapter)
 599 {
 600         struct beamforming_info *info;
 601         struct sounding_info *sounding;
 602         u8 idx;
 603
 604
 605         info = GET_BEAMFORM_INFO(adapter);
 606         sounding = &info->sounding_info;
 607
 608         _rtw_memset(sounding->su_sounding_list, 0xFF, MAX_NUM_BEAMFORMEE_SU);
 609         _rtw_memset(sounding->mu_sounding_list, 0xFF, MAX_NUM_BEAMFORMEE_MU);
 610         sounding->su_bfee_curidx = 0xFF;
 611         sounding->candidate_mu_bfee_cnt = 0;
 612
 613         /* Clear bSound flag for the new period */
 614         for (idx = 0; idx < MAX_BEAMFORMEE_ENTRY_NUM; idx++) {
 615                 if ((info->bfee_entry[idx].used == _TRUE)
 616                     && (info->bfee_entry[idx].sounding == _TRUE)) {
 617                         info->bfee_entry[idx].sounding = _FALSE;
 618                         info->bfee_entry[idx].bCandidateSoundingPeer = _FALSE;
 619                 }
 620         }
 621 }
 622
 623 /*
 624  * Return
 625  *      0       Prepare sounding list OK
 626  *      -1      Fail to prepare sounding list, because no beamformee need to souding
 627  *      -2      Fail to prepare sounding list, because beamformee state not ready
 628  *
 629  */
 630 static int _sounding_get_list(PADAPTER adapter)
 631 {
 632         struct beamforming_info *info;
 633         struct sounding_info *sounding;
 634         struct beamformee_entry *bfee;
 635         u8 i, mu_idx = 0, su_idx = 0, not_ready = 0;
 636         int ret = 0;
 637
 638
 639         info = GET_BEAMFORM_INFO(adapter);
 640         sounding = &info->sounding_info;
 641
 642         /* Add MU BFee list first because MU priority is higher than SU */
 643         for (i = 0; i < MAX_BEAMFORMEE_ENTRY_NUM; i++) {
 644                 bfee = &info->bfee_entry[i];
 645                 if (bfee->used == _FALSE)
 646                         continue;
 647
 648                 if (bfee->state != BEAMFORM_ENTRY_HW_STATE_ADDED) {
 649                         RTW_ERR("%s: Invalid BFee idx(%d) Hw state=%d\n", __FUNCTION__, i, bfee->state);
 650                         not_ready++;
 651                         continue;
 652                 }
 653
 654                 /*
 655                  * Decrease BFee's SoundCnt per period
 656                  * If the remain count is 0,
 657                  * then it can be sounded at this time
 658                  */
 659                 if (bfee->SoundCnt) {
 660                         bfee->SoundCnt--;
 661                         if (bfee->SoundCnt)
 662                                 continue;
 663                 }
 664
 665                 /*
 666                  * <tynli_Note>
 667                  *      If the STA supports MU BFee capability then we add it to MUSoundingList directly
 668                  *      because we can only sound one STA by unicast NDPA with MU cap enabled to get correct channel info.
 669                  *      Suggested by BB team Luke Lee. 2015.11.25.
 670                  */
 671                 if (bfee->cap & BEAMFORMEE_CAP_VHT_MU) {
 672                         /* MU BFee */
 673                         if (mu_idx >= MAX_NUM_BEAMFORMEE_MU) {
 674                                 RTW_ERR("%s: Too much MU bfee entry(Limit:%d)\n", __FUNCTION__, MAX_NUM_BEAMFORMEE_MU);
 675                                 continue;
 676                         }
 677
 678                         if (bfee->bApplySounding == _TRUE) {
 679                                 bfee->bCandidateSoundingPeer = _TRUE;
 680                                 bfee->SoundCnt = GetInitSoundCnt(bfee->sound_period, sounding->min_sounding_period);
 681                                 sounding->mu_sounding_list[mu_idx] = i;
 682                                 mu_idx++;
 683                         }
 684                 } else if (bfee->cap & (BEAMFORMEE_CAP_VHT_SU|BEAMFORMEE_CAP_HT_EXPLICIT)) {
 685                         /* SU BFee (HT/VHT) */
 686                         if (su_idx >= MAX_NUM_BEAMFORMEE_SU) {
 687                                 RTW_ERR("%s: Too much SU bfee entry(Limit:%d)\n", __FUNCTION__, MAX_NUM_BEAMFORMEE_SU);
 688                                 continue;
 689                         }
 690
 691                         if (bfee->bDeleteSounding == _TRUE) {
 692                                 sounding->su_sounding_list[su_idx] = i;
 693                                 su_idx++;
 694                         } else if ((bfee->bApplySounding == _TRUE)
 695                             && (bfee->bSuspendSUCap == _FALSE)) {
 696                                 bfee->bCandidateSoundingPeer = _TRUE;
 697                                 bfee->SoundCnt = GetInitSoundCnt(bfee->sound_period, sounding->min_sounding_period);
 698                                 sounding->su_sounding_list[su_idx] = i;
 699                                 su_idx++;
 700                         }
 701                 }
 702         }
 703
 704         sounding->candidate_mu_bfee_cnt = mu_idx;
 705
 706         if (su_idx + mu_idx == 0) {
 707                 ret = -1;
 708                 if (not_ready)
 709                         ret = -2;
 710         }
 711
 712         RTW_INFO("-%s: There are %d SU and %d MU BFees in this sounding period\n", __FUNCTION__, su_idx, mu_idx);
 713
 714         return ret;
 715 }
 716
 717 static void _sounding_handler(PADAPTER adapter)
 718 {
 719         struct beamforming_info *info;
 720         struct sounding_info *sounding;
 721         struct beamformee_entry *bfee;
 722         u8 su_idx, i;
 723         u32 timeout_period = 0;
 724         u8 set_timer = _FALSE;
 725         int ret = 0;
 726         static u16 wait_cnt = 0;
 727
 728
 729         info = GET_BEAMFORM_INFO(adapter);
 730         sounding = &info->sounding_info;
 731
 732         RTW_DBG("+%s: state=%d\n", __FUNCTION__, sounding->state);
 733         if ((sounding->state != SOUNDING_STATE_INIT)
 734             && (sounding->state != SOUNDING_STATE_SU_SOUNDDOWN)
 735             && (sounding->state != SOUNDING_STATE_MU_SOUNDDOWN)
 736             && (sounding->state != SOUNDING_STATE_SOUNDING_TIMEOUT)) {
 737                 RTW_WARN("%s: Invalid State(%d) and return!\n", __FUNCTION__, sounding->state);
 738                 return;
 739         }
 740
 741         if (sounding->state == SOUNDING_STATE_INIT) {
 742                 RTW_INFO("%s: Sounding start\n", __FUNCTION__);
 743
 744                 /* Init Var */
 745                 _sounding_reset_vars(adapter);
 746
 747                 /* Get the sounding list of this sounding period */
 748                 ret = _sounding_get_list(adapter);
 749                 if (ret == -1) {
 750                         wait_cnt = 0;
 751                         sounding->state = SOUNDING_STATE_NONE;
 752                         RTW_ERR("%s: No BFees found, set to SOUNDING_STATE_NONE\n", __FUNCTION__);
 753                         info->sounding_running--;
 754                         return;
 755                 }
 756                 if (ret == -2) {
 757                         RTW_WARN("%s: Temporarily cann't find BFee to sounding\n", __FUNCTION__);
 758                         if (wait_cnt < 5) {
 759                                 wait_cnt++;
 760                         } else {
 761                                 wait_cnt = 0;
 762                                 sounding->state = SOUNDING_STATE_NONE;
 763                                 RTW_ERR("%s: Wait changing state timeout!! Set to SOUNDING_STATE_NONE\n", __FUNCTION__);
 764                         }
 765                         info->sounding_running--;
 766                         return;
 767                 }
 768                 if (ret != 0) {
 769                         wait_cnt = 0;
 770                         RTW_ERR("%s: Unkown state(%d)!\n", __FUNCTION__, ret);
 771                         info->sounding_running--;
 772                         return;
 773
 774                 }
 775
 776                 wait_cnt = 0;
 777
 778                 if (check_fwstate(&adapter->mlmepriv, WIFI_SITE_MONITOR) == _TRUE) {
 779                         RTW_INFO("%s: Sounding abort! scanning APs...\n", __FUNCTION__);
 780                         info->sounding_running--;
 781                         return;
 782                 }
 783
 784                 rtw_ps_deny(adapter, PS_DENY_BEAMFORMING);
 785                 LeaveAllPowerSaveModeDirect(adapter);
 786         }
 787
 788         /* Get non-sound SU BFee index */
 789         for (i = 0; i < MAX_NUM_BEAMFORMEE_SU; i++) {
 790                 su_idx = sounding->su_sounding_list[i];
 791                 if (su_idx >= MAX_BEAMFORMEE_ENTRY_NUM)
 792                         continue;
 793                 bfee = &info->bfee_entry[su_idx];
 794                 if (_FALSE == bfee->sounding)
 795                         break;
 796         }
 797         if (i < MAX_NUM_BEAMFORMEE_SU) {
 798                 sounding->su_bfee_curidx = su_idx;
 799                 /* Set to sounding start state */
 800                 sounding->state = SOUNDING_STATE_SU_START;
 801                 RTW_DBG("%s: Set to SOUNDING_STATE_SU_START\n", __FUNCTION__);
 802
 803                 bfee->sounding = _TRUE;
 804                 /* Reset sounding timeout flag for the new sounding */
 805                 bfee->bSoundingTimeout = _FALSE;
 806
 807                 if (_TRUE == bfee->bDeleteSounding) {
 808                         u8 res = _FALSE;
 809                         rtw_bf_cmd(adapter, BEAMFORMING_CTRL_END_PERIOD, &res, 1, 0);
 810                         return;
 811                 }
 812
 813                 /* Start SU sounding */
 814                 if (bfee->cap & BEAMFORMEE_CAP_VHT_SU)
 815                         _send_vht_ndpa_packet(adapter, bfee->mac_addr, bfee->aid, bfee->sound_bw);
 816                 else if (bfee->cap & BEAMFORMEE_CAP_HT_EXPLICIT)
 817                         _send_ht_ndpa_packet(adapter, bfee->mac_addr, bfee->sound_bw);
 818
 819                 /* Set sounding timeout timer */
 820                 _set_timer(&info->sounding_timeout_timer, SU_SOUNDING_TIMEOUT);
 821                 return;
 822         }
 823
 824         if (sounding->candidate_mu_bfee_cnt > 0) {
 825                 /*
 826                  * If there is no SU BFee then find MU BFee and perform MU sounding
 827                  *
 828                  * <tynli_note> Need to check the MU starting condition. 2015.12.15.
 829                  */
 830                 sounding->state = SOUNDING_STATE_MU_START;
 831                 RTW_DBG("%s: Set to SOUNDING_STATE_MU_START\n", __FUNCTION__);
 832
 833                 /* Update MU BFee info */
 834                 for (i = 0; i < sounding->candidate_mu_bfee_cnt; i++) {
 835                         bfee = &info->bfee_entry[sounding->mu_sounding_list[i]];
 836                         bfee->sounding = _TRUE;
 837                 }
 838
 839                 /* Send MU NDPA */
 840                 bfee = &info->bfee_entry[sounding->mu_sounding_list[0]];
 841                 _send_vht_mu_ndpa_packet(adapter, bfee->sound_bw);
 842
 843                 /* Send BF report poll if more than 1 MU STA */
 844                 for (i = 1; i < sounding->candidate_mu_bfee_cnt; i++) {
 845                         bfee = &info->bfee_entry[sounding->mu_sounding_list[i]];
 846
 847                         if (i == (sounding->candidate_mu_bfee_cnt - 1))/* The last STA*/
 848                                 _send_bf_report_poll(adapter, bfee->mac_addr, _TRUE);
 849                         else
 850                                 _send_bf_report_poll(adapter, bfee->mac_addr, _FALSE);
 851                 }
 852
 853                 sounding->candidate_mu_bfee_cnt = 0;
 854
 855                 /* Set sounding timeout timer */
 856                 _set_timer(&info->sounding_timeout_timer, MU_SOUNDING_TIMEOUT);
 857                 return;
 858         }
 859
 860         info->sounding_running--;
 861         sounding->state = SOUNDING_STATE_INIT;
 862         RTW_INFO("%s: Sounding finished!\n", __FUNCTION__);
 863         rtw_ps_deny_cancel(adapter, PS_DENY_BEAMFORMING);
 864 }
 865
 866 static void _sounding_force_stop(PADAPTER adapter)
 867 {
 868         struct beamforming_info *info;
 869         struct sounding_info *sounding;
 870         u8 cancelled;
 871
 872
 873         info = GET_BEAMFORM_INFO(adapter);
 874         sounding = &info->sounding_info;
 875
 876         if ((sounding->state == SOUNDING_STATE_SU_START)
 877             || (sounding->state == SOUNDING_STATE_MU_START)) {
 878                 u8 res = _FALSE;
 879                 _cancel_timer(&info->sounding_timeout_timer, &cancelled);
 880                 rtw_bf_cmd(adapter, BEAMFORMING_CTRL_END_PERIOD, &res, 1, 1);
 881                 return;
 882         }
 883
 884         info->sounding_running--;
 885         sounding->state = SOUNDING_STATE_INIT;
 886         RTW_INFO("%s: Sounding finished!\n", __FUNCTION__);
 887         rtw_ps_deny_cancel(adapter, PS_DENY_BEAMFORMING);
 888 }
 889
 890 static void _sounding_timer_handler(void *FunctionContext)
 891 {
 892         PADAPTER adapter;
 893         struct beamforming_info *info;
 894         struct sounding_info *sounding;
 895         static u8 delay = 0;
 896
 897
 898         RTW_DBG("+%s\n", __FUNCTION__);
 899
 900         adapter = (PADAPTER)FunctionContext;
 901         info = GET_BEAMFORM_INFO(adapter);
 902         sounding = &info->sounding_info;
 903
 904         if (SOUNDING_STATE_NONE == sounding->state) {
 905                 RTW_INFO("%s: Stop!\n", __FUNCTION__);
 906                 if (info->sounding_running)
 907                         RTW_WARN("%s: souding_running=%d when thread stop!\n",
 908                                  __FUNCTION__, info->sounding_running);
 909                 return;
 910         }
 911
 912         _set_timer(&info->sounding_timer, sounding->min_sounding_period);
 913
 914         if (!info->sounding_running) {
 915                 if (SOUNDING_STATE_INIT != sounding->state) {
 916                         RTW_WARN("%s: state(%d) != SOUNDING_STATE_INIT!!\n", __FUNCTION__, sounding->state);
 917                         sounding->state = SOUNDING_STATE_INIT;
 918                 }
 919                 delay = 0;
 920                 info->sounding_running++;
 921                 rtw_bf_cmd(adapter, BEAMFORMING_CTRL_START_PERIOD, NULL, 0, 1);
 922         } else {
 923                 if (delay != 0xFF)
 924                         delay++;
 925                 RTW_WARN("%s: souding is still processing...(state:%d, running:%d, delay:%d)\n",
 926                          __FUNCTION__, sounding->state, info->sounding_running, delay);
 927                 if (delay > 3) {
 928                         RTW_WARN("%s: Stop sounding!!\n", __FUNCTION__);
 929                         _sounding_force_stop(adapter);
 930                 }
 931         }
 932 }
 933
 934 static void _sounding_timeout_timer_handler(void *FunctionContext)
 935 {
 936         PADAPTER adapter;
 937         struct beamforming_info *info;
 938         struct sounding_info *sounding;
 939         struct beamformee_entry *bfee;
 940
 941
 942         RTW_WARN("+%s\n", __FUNCTION__);
 943
 944         adapter = (PADAPTER)FunctionContext;
 945         info = GET_BEAMFORM_INFO(adapter);
 946         sounding = &info->sounding_info;
 947
 948         if (SOUNDING_STATE_SU_START == sounding->state) {
 949                 sounding->state = SOUNDING_STATE_SOUNDING_TIMEOUT;
 950                 RTW_ERR("%s: Set to SU SOUNDING_STATE_SOUNDING_TIMEOUT\n", __FUNCTION__);
 951                 /* SU BFee */
 952                 bfee = &info->bfee_entry[sounding->su_bfee_curidx];
 953                 bfee->bSoundingTimeout = _TRUE;
 954                 RTW_WARN("%s: The BFee entry[%d] is Sounding Timeout!\n", __FUNCTION__, sounding->su_bfee_curidx);
 955         } else if (SOUNDING_STATE_MU_START == sounding->state) {
 956                 sounding->state = SOUNDING_STATE_SOUNDING_TIMEOUT;
 957                 RTW_ERR("%s: Set to MU SOUNDING_STATE_SOUNDING_TIMEOUT\n", __FUNCTION__);
 958         } else {
 959                 RTW_WARN("%s: unexpected sounding state:0x%02x\n", __FUNCTION__, sounding->state);
 960                 return;
 961         }
 962
 963         rtw_bf_cmd(adapter, BEAMFORMING_CTRL_START_PERIOD, NULL, 0, 1);
 964 }
 965
 966 static struct beamformer_entry *_bfer_get_free_entry(PADAPTER adapter)
 967 {
 968         u8 i = 0;
 969         struct beamforming_info *info;
 970         struct beamformer_entry *bfer;
 971
 972
 973         info = GET_BEAMFORM_INFO(adapter);
 974
 975         for (i = 0; i < MAX_BEAMFORMER_ENTRY_NUM; i++) {
 976                 bfer = &info->bfer_entry[i];
 977                 if (bfer->used == _FALSE)
 978                         return bfer;
 979         }
 980
 981         return NULL;
 982 }
 983
 984 static struct beamformer_entry *_bfer_get_entry_by_addr(PADAPTER adapter, u8 *ra)
 985 {
 986         u8 i = 0;
 987         struct beamforming_info *info;
 988         struct beamformer_entry *bfer;
 989
 990
 991         info = GET_BEAMFORM_INFO(adapter);
 992
 993         for (i = 0; i < MAX_BEAMFORMER_ENTRY_NUM; i++) {
 994                 bfer = &info->bfer_entry[i];
 995                 if (bfer->used == _FALSE)
 996                         continue;
 997                 if (_rtw_memcmp(ra, bfer->mac_addr, ETH_ALEN) == _TRUE)
 998                         return bfer;
 999         }
1000
1001         return NULL;
1002 }
1003
1004 static struct beamformer_entry *_bfer_add_entry(PADAPTER adapter,
1005         struct sta_info *sta, u8 bf_cap, u8 sounding_dim, u8 comp_steering)
1006 {
1007         struct mlme_priv *mlme;
1008         struct beamforming_info *info;
1009         struct beamformer_entry *bfer;
1010         u8 *bssid;
1011         u16 val16;
1012         u8 i;
1013
1014
1015         mlme = &adapter->mlmepriv;
1016         info = GET_BEAMFORM_INFO(adapter);
1017
1018         bfer = _bfer_get_entry_by_addr(adapter, sta->hwaddr);
1019         if (!bfer) {
1020                 bfer = _bfer_get_free_entry(adapter);
1021                 if (!bfer)
1022                         return NULL;
1023         }
1024
1025         bfer->used = _TRUE;
1026         _get_txvector_parameter(adapter, sta, &bfer->g_id, &bfer->p_aid);
1027         _rtw_memcpy(bfer->mac_addr, sta->hwaddr, ETH_ALEN);
1028         bfer->cap = bf_cap;
1029         bfer->state = BEAMFORM_ENTRY_HW_STATE_ADD_INIT;
1030         bfer->NumofSoundingDim = sounding_dim;
1031
1032         if (TEST_FLAG(bf_cap, BEAMFORMER_CAP_VHT_MU)) {
1033                 info->beamformer_mu_cnt += 1;
1034                 bfer->aid = sta->aid;
1035         } else if (TEST_FLAG(bf_cap, BEAMFORMER_CAP_VHT_SU|BEAMFORMER_CAP_HT_EXPLICIT)) {
1036                 info->beamformer_su_cnt += 1;
1037
1038                 /* Record HW idx info */
1039                 for (i = 0; i < MAX_NUM_BEAMFORMER_SU; i++) {
1040                         if ((info->beamformer_su_reg_maping & BIT(i)) == 0) {
1041                                 info->beamformer_su_reg_maping |= BIT(i);
1042                                 bfer->su_reg_index = i;
1043                                 break;
1044                         }
1045                 }
1046                 RTW_INFO("%s: Add BFer entry beamformer_su_reg_maping=%#x, su_reg_index=%d\n",
1047                          __FUNCTION__, info->beamformer_su_reg_maping, bfer->su_reg_index);
1048         }
1049
1050         return bfer;
1051 }
1052
1053 static void _bfer_remove_entry(PADAPTER adapter, struct beamformer_entry *entry)
1054 {
1055         struct beamforming_info *info;
1056
1057
1058         info = GET_BEAMFORM_INFO(adapter);
1059
1060         entry->state = BEAMFORM_ENTRY_HW_STATE_DELETE_INIT;
1061
1062         if (TEST_FLAG(entry->cap, BEAMFORMER_CAP_VHT_MU)) {
1063                 info->beamformer_mu_cnt -= 1;
1064                 _rtw_memset(entry->gid_valid, 0, 8);
1065                 _rtw_memset(entry->user_position, 0, 16);
1066         } else if (TEST_FLAG(entry->cap, BEAMFORMER_CAP_VHT_SU|BEAMFORMER_CAP_HT_EXPLICIT)) {
1067                 info->beamformer_su_cnt -= 1;
1068         }
1069
1070         if (info->beamformer_mu_cnt == 0)
1071                 info->beamforming_cap &= ~BEAMFORMEE_CAP_VHT_MU;
1072         if (info->beamformer_su_cnt == 0)
1073                 info->beamforming_cap &= ~(BEAMFORMEE_CAP_VHT_SU|BEAMFORMEE_CAP_HT_EXPLICIT);
1074 }
1075
1076 static u8 _bfer_set_entry_gid(PADAPTER adapter, u8 *addr, u8 *gid, u8 *position)
1077 {
1078         struct beamformer_entry *bfer = NULL;
1079
1080
1081         bfer = _bfer_get_entry_by_addr(adapter, addr);
1082         if (!bfer) {
1083                 RTW_INFO("%s: Cannot find BFer entry!!\n", __FUNCTION__);
1084                 return _FAIL;
1085         }
1086
1087         /* Parsing Membership Status Array */
1088         _rtw_memcpy(bfer->gid_valid, gid, 8);
1089         /* Parsing User Position Array */
1090         _rtw_memcpy(bfer->user_position, position, 16);
1091
1092         /* Config HW GID table */
1093         rtw_bf_cmd(adapter, BEAMFORMING_CTRL_SET_GID_TABLE, (u8*)&bfer, sizeof(struct beamformer_entry *), 1);
1094
1095         return _SUCCESS;
1096 }
1097
1098 static struct beamformee_entry *_bfee_get_free_entry(PADAPTER adapter)
1099 {
1100         u8 i = 0;
1101         struct beamforming_info *info;
1102         struct beamformee_entry *bfee;
1103
1104
1105         info = GET_BEAMFORM_INFO(adapter);
1106
1107         for (i = 0; i < MAX_BEAMFORMEE_ENTRY_NUM; i++) {
1108                 bfee = &info->bfee_entry[i];
1109                 if (bfee->used == _FALSE)
1110                         return bfee;
1111         }
1112
1113         return NULL;
1114 }
1115
1116 static struct beamformee_entry *_bfee_get_entry_by_addr(PADAPTER adapter, u8 *ra)
1117 {
1118         u8 i = 0;
1119         struct beamforming_info *info;
1120         struct beamformee_entry *bfee;
1121
1122
1123         info = GET_BEAMFORM_INFO(adapter);
1124
1125         for (i = 0; i < MAX_BEAMFORMEE_ENTRY_NUM; i++) {
1126                 bfee = &info->bfee_entry[i];
1127                 if (bfee->used == _FALSE)
1128                         continue;
1129                 if (_rtw_memcmp(ra, bfee->mac_addr, ETH_ALEN) == _TRUE)
1130                         return bfee;
1131         }
1132
1133         return NULL;
1134 }
1135
1136 static u8 _bfee_get_first_su_entry_idx(PADAPTER adapter, struct beamformee_entry *ignore)
1137 {
1138         struct beamforming_info *info;
1139         struct beamformee_entry *bfee;
1140         u8 i;
1141
1142
1143         info = GET_BEAMFORM_INFO(adapter);
1144
1145         for (i = 0; i < MAX_BEAMFORMEE_ENTRY_NUM; i++) {
1146                 bfee = &info->bfee_entry[i];
1147                 if (ignore && (bfee == ignore))
1148                         continue;
1149                 if (bfee->used == _FALSE)
1150                         continue;
1151                 if ((!TEST_FLAG(bfee->cap, BEAMFORMEE_CAP_VHT_MU))
1152                     && TEST_FLAG(bfee->cap, BEAMFORMEE_CAP_VHT_SU|BEAMFORMEE_CAP_HT_EXPLICIT))
1153                         return i;
1154         }
1155
1156         return 0xFF;
1157 }
1158
1159 /*
1160  * Description:
1161  *      Get the first entry index of MU Beamformee.
1162  *
1163  * Return Value:
1164  *      Index of the first MU sta, or 0xFF for invalid index.
1165  *
1166  * 2015.05.25. Created by tynli.
1167  *
1168  */
1169 static u8 _bfee_get_first_mu_entry_idx(PADAPTER adapter, struct beamformee_entry *ignore)
1170 {
1171         struct beamforming_info *info;
1172         struct beamformee_entry *bfee;
1173         u8 i;
1174
1175
1176         info = GET_BEAMFORM_INFO(adapter);
1177
1178         for (i = 0; i < MAX_BEAMFORMEE_ENTRY_NUM; i++) {
1179                 bfee = &info->bfee_entry[i];
1180                 if (ignore && (bfee == ignore))
1181                         continue;
1182                 if (bfee->used == _FALSE)
1183                         continue;
1184                 if (TEST_FLAG(bfee->cap, BEAMFORMEE_CAP_VHT_MU))
1185                         return i;
1186         }
1187
1188         return 0xFF;
1189 }
1190
1191 static struct beamformee_entry *_bfee_add_entry(PADAPTER adapter,
1192         struct sta_info *sta, u8 bf_cap, u8 sounding_dim, u8 comp_steering)
1193 {
1194         struct mlme_priv *mlme;
1195         struct beamforming_info *info;
1196         struct beamformee_entry *bfee;
1197         u8 *bssid;
1198         u16 val16;
1199         u8 i;
1200
1201
1202         mlme = &adapter->mlmepriv;
1203         info = GET_BEAMFORM_INFO(adapter);
1204
1205         bfee = _bfee_get_entry_by_addr(adapter, sta->hwaddr);
1206         if (!bfee) {
1207                 bfee = _bfee_get_free_entry(adapter);
1208                 if (!bfee)
1209                         return NULL;
1210         }
1211
1212         bfee->used = _TRUE;
1213         bfee->aid = sta->aid;
1214         bfee->mac_id = sta->mac_id;
1215         bfee->sound_bw = sta->bw_mode;
1216
1217         _get_txvector_parameter(adapter, sta, &bfee->g_id, &bfee->p_aid);
1218         sta->txbf_gid = bfee->g_id;
1219         sta->txbf_paid = bfee->p_aid;
1220
1221         _rtw_memcpy(bfee->mac_addr, sta->hwaddr, ETH_ALEN);
1222         bfee->txbf = _FALSE;
1223         bfee->sounding = _FALSE;
1224         bfee->sound_period = 40;
1225         _sounding_update_min_period(adapter, bfee->sound_period, _FALSE);
1226         bfee->SoundCnt = GetInitSoundCnt(bfee->sound_period, info->sounding_info.min_sounding_period);
1227         bfee->cap = bf_cap;
1228         bfee->state = BEAMFORM_ENTRY_HW_STATE_ADD_INIT;
1229
1230         bfee->bCandidateSoundingPeer = _FALSE;
1231         bfee->bSoundingTimeout = _FALSE;
1232         bfee->bDeleteSounding = _FALSE;
1233         bfee->bApplySounding = _TRUE;
1234
1235         bfee->tx_timestamp = 0;
1236         bfee->tx_bytes = 0;
1237
1238         bfee->LogStatusFailCnt = 0;
1239         bfee->NumofSoundingDim = sounding_dim;
1240         bfee->comp_steering_num_of_bfer = comp_steering;
1241         bfee->bSuspendSUCap = _FALSE;
1242
1243         if (TEST_FLAG(bf_cap, BEAMFORMEE_CAP_VHT_MU)) {
1244                 info->beamformee_mu_cnt += 1;
1245                 info->first_mu_bfee_index = _bfee_get_first_mu_entry_idx(adapter, NULL);
1246
1247                 if (_TRUE == info->bEnableSUTxBFWorkAround) {
1248                         /* When the first MU BFee added, discard SU BFee bfee's capability */
1249                         if ((info->beamformee_mu_cnt == 1) && (info->beamformee_su_cnt > 0)) {
1250                                 if (info->TargetSUBFee) {
1251                                         info->TargetSUBFee->bSuspendSUCap = _TRUE;
1252                                         info->TargetSUBFee->bDeleteSounding = _TRUE;
1253                                 } else {
1254                                         RTW_ERR("%s: UNEXPECTED!! info->TargetSUBFee is NULL!", __FUNCTION__);
1255                                 }
1256                                 info->TargetSUBFee = NULL;
1257                                 _rtw_memset(&info->TargetCSIInfo, 0, sizeof(struct _RT_CSI_INFO));
1258                                 rtw_bf_cmd(adapter, BEAMFORMING_CTRL_SET_CSI_REPORT, (u8*)&info->TargetCSIInfo, sizeof(struct _RT_CSI_INFO), 0);
1259                         }
1260                 }
1261
1262                 /* Record HW idx info */
1263                 for (i = 0; i < MAX_NUM_BEAMFORMEE_MU; i++) {
1264                         if ((info->beamformee_mu_reg_maping & BIT(i)) == 0) {
1265                                 info->beamformee_mu_reg_maping |= BIT(i);
1266                                 bfee->mu_reg_index = i;
1267                                 break;
1268                         }
1269                 }
1270                 RTW_INFO("%s: Add BFee entry beamformee_mu_reg_maping=%#x, mu_reg_index=%d\n",
1271                          __FUNCTION__, info->beamformee_mu_reg_maping, bfee->mu_reg_index);
1272
1273         } else if (TEST_FLAG(bf_cap, BEAMFORMEE_CAP_VHT_SU|BEAMFORMEE_CAP_HT_EXPLICIT)) {
1274                 info->beamformee_su_cnt += 1;
1275
1276                 if (_TRUE == info->bEnableSUTxBFWorkAround) {
1277                         /* Record the first SU BFee index. We only allow the first SU BFee to be sound */
1278                         if ((info->beamformee_su_cnt == 1) && (info->beamformee_mu_cnt == 0)) {
1279                                 info->TargetSUBFee = bfee;
1280                                 _rtw_memset(&info->TargetCSIInfo, 0, sizeof(struct _RT_CSI_INFO));
1281                                 bfee->bSuspendSUCap = _FALSE;
1282                         } else {
1283                                 bfee->bSuspendSUCap = _TRUE;
1284                         }
1285                 }
1286
1287                 /* Record HW idx info */
1288                 for (i = 0; i < MAX_NUM_BEAMFORMEE_SU; i++) {
1289                         if ((info->beamformee_su_reg_maping & BIT(i)) == 0) {
1290                                 info->beamformee_su_reg_maping |= BIT(i);
1291                                 bfee->su_reg_index = i;
1292                                 break;
1293                         }
1294                 }
1295                 RTW_INFO("%s: Add BFee entry beamformee_su_reg_maping=%#x, su_reg_index=%d\n",
1296                          __FUNCTION__, info->beamformee_su_reg_maping, bfee->su_reg_index);
1297         }
1298
1299         return bfee;
1300 }
1301
1302 static void _bfee_remove_entry(PADAPTER adapter, struct beamformee_entry *entry)
1303 {
1304         struct beamforming_info *info;
1305         u8 idx;
1306
1307
1308         info = GET_BEAMFORM_INFO(adapter);
1309
1310         entry->state = BEAMFORM_ENTRY_HW_STATE_DELETE_INIT;
1311
1312         if (TEST_FLAG(entry->cap, BEAMFORMEE_CAP_VHT_MU)) {
1313                 info->beamformee_mu_cnt -= 1;
1314                 info->first_mu_bfee_index = _bfee_get_first_mu_entry_idx(adapter, entry);
1315
1316                 if (_TRUE == info->bEnableSUTxBFWorkAround) {
1317                         if ((info->beamformee_mu_cnt == 0) && (info->beamformee_su_cnt > 0)) {
1318                                 idx = _bfee_get_first_su_entry_idx(adapter, NULL);
1319                                 info->TargetSUBFee = &info->bfee_entry[idx];
1320                                 _rtw_memset(&info->TargetCSIInfo, 0, sizeof(struct _RT_CSI_INFO));
1321                                 info->TargetSUBFee->bSuspendSUCap = _FALSE;
1322                         }
1323                 }
1324         } else if (TEST_FLAG(entry->cap, BEAMFORMEE_CAP_VHT_SU|BEAMFORMEE_CAP_HT_EXPLICIT)) {
1325                 info->beamformee_su_cnt -= 1;
1326
1327                 /* When the target SU BFee leaves, disable workaround */
1328                 if ((_TRUE == info->bEnableSUTxBFWorkAround)
1329                     && (entry == info->TargetSUBFee)) {
1330                         entry->bSuspendSUCap = _TRUE;
1331                         info->TargetSUBFee = NULL;
1332                         _rtw_memset(&info->TargetCSIInfo, 0, sizeof(struct _RT_CSI_INFO));
1333                         rtw_bf_cmd(adapter, BEAMFORMING_CTRL_SET_CSI_REPORT, (u8*)&info->TargetCSIInfo, sizeof(struct _RT_CSI_INFO), 0);
1334                 }
1335         }
1336
1337         if (info->beamformee_mu_cnt == 0)
1338                 info->beamforming_cap &= ~BEAMFORMER_CAP_VHT_MU;
1339         if (info->beamformee_su_cnt == 0)
1340                 info->beamforming_cap &= ~(BEAMFORMER_CAP_VHT_SU|BEAMFORMER_CAP_HT_EXPLICIT);
1341
1342         _sounding_update_min_period(adapter, 0, _TRUE);
1343 }
1344
1345 static enum beamforming_cap _bfee_get_entry_cap_by_macid(PADAPTER adapter, u8 macid)
1346 {
1347         struct beamforming_info *info;
1348         struct beamformee_entry *bfee;
1349         u8 i;
1350
1351
1352         info = GET_BEAMFORM_INFO(adapter);
1353
1354         for (i = 0; i < MAX_BEAMFORMER_ENTRY_NUM; i++) {
1355                 bfee = &info->bfee_entry[i];
1356                 if (bfee->used == _FALSE)
1357                         continue;
1358                 if (bfee->mac_id == macid)
1359                         return bfee->cap;
1360         }
1361
1362         return BEAMFORMING_CAP_NONE;
1363 }
1364
1365 static void _beamforming_enter(PADAPTER adapter, void *p)
1366 {
1367         struct mlme_priv *mlme;
1368         struct ht_priv *htpriv;
1369 #ifdef CONFIG_80211AC_VHT
1370         struct vht_priv *vhtpriv;
1371 #endif
1372         struct mlme_ext_priv *mlme_ext;
1373         struct sta_info *sta, *sta_copy;
1374         struct beamforming_info *info;
1375         struct beamformer_entry *bfer = NULL;
1376         struct beamformee_entry *bfee = NULL;
1377         u8 wireless_mode;
1378         u8 sta_bf_cap;
1379         u8 sounding_dim = 0; /* number of sounding dimensions */
1380         u8 comp_steering_num = 0; /* compressed steering number */
1381
1382
1383         mlme = &adapter->mlmepriv;
1384         htpriv = &mlme->htpriv;
1385 #ifdef CONFIG_80211AC_VHT
1386         vhtpriv = &mlme->vhtpriv;
1387 #endif
1388         mlme_ext = &adapter->mlmeextpriv;
1389         info = GET_BEAMFORM_INFO(adapter);
1390
1391         sta_copy = (struct sta_info *)p;
1392         sta = rtw_get_stainfo(&adapter->stapriv, sta_copy->hwaddr);
1393         if (!sta) {
1394                 RTW_ERR("%s: Cann't find STA info for " MAC_FMT "\n",
1395                         __FUNCTION__, MAC_ARG(sta_copy->hwaddr));
1396                 return;
1397         }
1398         if (sta != sta_copy) {
1399                 RTW_WARN("%s: Origin sta(fake)=%p realsta=%p for " MAC_FMT "\n",
1400                          __FUNCTION__, sta_copy, sta, MAC_ARG(sta_copy->hwaddr));
1401         }
1402
1403         /* The current setting does not support Beaforming */
1404         wireless_mode = sta->wireless_mode;
1405         if ((is_supported_ht(wireless_mode) == _FALSE)
1406             && (is_supported_vht(wireless_mode) == _FALSE)) {
1407                 RTW_WARN("%s: Not support HT or VHT mode\n", __FUNCTION__);
1408                 return;
1409         }
1410
1411         if ((0 == htpriv->beamform_cap)
1412 #ifdef CONFIG_80211AC_VHT
1413             && (0 == vhtpriv->beamform_cap)
1414 #endif
1415            ) {
1416                 RTW_INFO("The configuration disabled Beamforming! Skip...\n");
1417                 return;
1418         }
1419
1420         _get_sta_beamform_cap(adapter, sta,
1421                               &sta_bf_cap, &sounding_dim, &comp_steering_num);
1422         RTW_INFO("STA Beamforming Capability=0x%02X\n", sta_bf_cap);
1423         if (sta_bf_cap == BEAMFORMING_CAP_NONE)
1424                 return;
1425         if ((sta_bf_cap & BEAMFORMEE_CAP_HT_EXPLICIT)
1426             || (sta_bf_cap & BEAMFORMEE_CAP_VHT_SU)
1427             || (sta_bf_cap & BEAMFORMEE_CAP_VHT_MU))
1428                 sta_bf_cap |= BEAMFORMEE_CAP;
1429         if ((sta_bf_cap & BEAMFORMER_CAP_HT_EXPLICIT)
1430             || (sta_bf_cap & BEAMFORMER_CAP_VHT_SU)
1431             || (sta_bf_cap & BEAMFORMER_CAP_VHT_MU))
1432                 sta_bf_cap |= BEAMFORMER_CAP;
1433
1434         if (sta_bf_cap & BEAMFORMER_CAP) {
1435                 /* The other side is beamformer */
1436                 bfer = _bfer_add_entry(adapter, sta, sta_bf_cap, sounding_dim, comp_steering_num);
1437                 if (!bfer)
1438                         RTW_ERR("%s: Fail to allocate bfer entry!\n", __FUNCTION__);
1439         }
1440         if (sta_bf_cap & BEAMFORMEE_CAP) {
1441                 /* The other side is beamformee */
1442                 bfee = _bfee_add_entry(adapter, sta, sta_bf_cap, sounding_dim, comp_steering_num);
1443                 if (!bfee)
1444                         RTW_ERR("%s: Fail to allocate bfee entry!\n", __FUNCTION__);
1445         }
1446         if (!bfer && !bfee)
1447                 return;
1448
1449         rtw_hal_set_hwreg(adapter, HW_VAR_SOUNDING_ENTER, (u8*)sta);
1450
1451         /* Perform sounding if there is BFee */
1452         if ((info->beamformee_su_cnt != 0)
1453             || (info->beamformee_mu_cnt != 0)) {
1454                 if (SOUNDING_STATE_NONE == info->sounding_info.state) {
1455                         info->sounding_info.state = SOUNDING_STATE_INIT;
1456                         /* Start sounding after 2 sec */
1457                         _set_timer(&info->sounding_timer, 2000);
1458                 }
1459         }
1460 }
1461
1462 static void _beamforming_reset(PADAPTER adapter)
1463 {
1464         RTW_ERR("%s: Not ready!!\n", __FUNCTION__);
1465 }
1466
1467 static void _beamforming_leave(PADAPTER adapter, u8 *ra)
1468 {
1469         struct beamforming_info *info;
1470         struct beamformer_entry *bfer = NULL;
1471         struct beamformee_entry *bfee = NULL;
1472         u8 bHwStateAddInit = _FALSE;
1473
1474
1475         RTW_INFO("+%s\n", __FUNCTION__);
1476
1477         info = GET_BEAMFORM_INFO(adapter);
1478         bfer = _bfer_get_entry_by_addr(adapter, ra);
1479         bfee = _bfee_get_entry_by_addr(adapter, ra);
1480
1481         if (!bfer && !bfee) {
1482                 RTW_WARN("%s: " MAC_FMT " is neither beamforming ee or er!!\n",
1483                         __FUNCTION__, MAC_ARG(ra));
1484                 return;
1485         }
1486
1487         if (bfer)
1488                 _bfer_remove_entry(adapter, bfer);
1489
1490         if (bfee)
1491                 _bfee_remove_entry(adapter, bfee);
1492
1493         rtw_hal_set_hwreg(adapter, HW_VAR_SOUNDING_LEAVE, ra);
1494
1495         /* Stop sounding if there is no any BFee */
1496         if ((info->beamformee_su_cnt == 0)
1497             && (info->beamformee_mu_cnt == 0)) {
1498                 u8 cancelled;
1499                 _cancel_timer(&info->sounding_timer, &cancelled);
1500                 _sounding_init(&info->sounding_info);
1501         }
1502
1503         RTW_INFO("-%s\n", __FUNCTION__);
1504 }
1505
1506 static void _beamforming_sounding_down(PADAPTER adapter, u8 status)
1507 {
1508         struct beamforming_info *info;
1509         struct sounding_info *sounding;
1510         struct beamformee_entry *bfee;
1511
1512
1513         info = GET_BEAMFORM_INFO(adapter);
1514         sounding = &info->sounding_info;
1515
1516         RTW_INFO("+%s: sounding=%d, status=0x%02x\n", __FUNCTION__, sounding->state, status);
1517
1518         if (sounding->state == SOUNDING_STATE_MU_START) {
1519                 RTW_INFO("%s: MU sounding done\n", __FUNCTION__);
1520                 sounding->state = SOUNDING_STATE_MU_SOUNDDOWN;
1521                 RTW_INFO("%s: Set to SOUNDING_STATE_MU_SOUNDDOWN\n", __FUNCTION__);
1522                 info->SetHalSoundownOnDemandCnt++;
1523                 rtw_hal_set_hwreg(adapter, HW_VAR_SOUNDING_STATUS, &status);
1524         } else if (sounding->state == SOUNDING_STATE_SU_START) {
1525                 RTW_INFO("%s: SU entry[%d] sounding down\n", __FUNCTION__, sounding->su_bfee_curidx);
1526                 bfee = &info->bfee_entry[sounding->su_bfee_curidx];
1527                 sounding->state = SOUNDING_STATE_SU_SOUNDDOWN;
1528                 RTW_INFO("%s: Set to SOUNDING_STATE_SU_SOUNDDOWN\n", __FUNCTION__);
1529
1530                 /*
1531                  * <tynli_note>
1532                  *      bfee->bSoundingTimeout this flag still cannot avoid
1533                  *      old sound down event happens in the new sounding period.
1534                  *      2015.12.10
1535                  */
1536                 if (_TRUE == bfee->bSoundingTimeout) {
1537                         RTW_WARN("%s: The entry[%d] is bSoundingTimeout!\n", __FUNCTION__, sounding->su_bfee_curidx);
1538                         bfee->bSoundingTimeout = _FALSE;
1539                         return;
1540                 }
1541
1542                 if (_TRUE == status) {
1543                         /* success */
1544                         bfee->LogStatusFailCnt = 0;
1545                         info->SetHalSoundownOnDemandCnt++;
1546                         rtw_hal_set_hwreg(adapter, HW_VAR_SOUNDING_STATUS, &status);
1547                 } else if (_TRUE == bfee->bDeleteSounding) {
1548                         RTW_WARN("%s: Delete entry[%d] sounding info!\n", __FUNCTION__, sounding->su_bfee_curidx);
1549                         rtw_hal_set_hwreg(adapter, HW_VAR_SOUNDING_STATUS, &status);
1550                         bfee->bDeleteSounding = _FALSE;
1551                 } else {
1552                         bfee->LogStatusFailCnt++;
1553                         RTW_WARN("%s: LogStatusFailCnt=%d\n", __FUNCTION__, bfee->LogStatusFailCnt);
1554                         if (bfee->LogStatusFailCnt > 30) {
1555                                 RTW_ERR("%s: LogStatusFailCnt > 30, Stop SOUNDING!!\n", __FUNCTION__);
1556                                 rtw_bf_cmd(adapter, BEAMFORMING_CTRL_LEAVE, bfee->mac_addr, ETH_ALEN, 1);
1557                         }
1558                 }
1559         } else {
1560                 RTW_WARN("%s: unexpected sounding state:0x%02x\n", __FUNCTION__, sounding->state);
1561                 return;
1562         }
1563
1564         rtw_bf_cmd(adapter, BEAMFORMING_CTRL_START_PERIOD, NULL, 0, 0);
1565 }
1566
1567 static void _c2h_snd_txbf(PADAPTER adapter, u8 *buf, u8 buf_len)
1568 {
1569         struct beamforming_info *info;
1570         u8 cancelled;
1571         u8 res;
1572
1573
1574         info = GET_BEAMFORM_INFO(adapter);
1575
1576         _cancel_timer(&info->sounding_timeout_timer, &cancelled);
1577
1578         res = C2H_SND_TXBF_GET_SND_RESULT(buf) ? _TRUE : _FALSE;
1579         RTW_INFO("+%s: %s\n", __FUNCTION__, res==_TRUE?"Success":"Fail!");
1580
1581         rtw_bf_cmd(adapter, BEAMFORMING_CTRL_END_PERIOD, &res, 1, 1);
1582 }
1583
1584 /*
1585  * Description:
1586  *      This function is for phydm only
1587  */
1588 enum beamforming_cap rtw_bf_bfee_get_entry_cap_by_macid(void *mlme, u8 macid)
1589 {
1590         PADAPTER adapter;
1591         enum beamforming_cap cap = BEAMFORMING_CAP_NONE;
1592
1593
1594         adapter = mlme_to_adapter((struct mlme_priv *)mlme);
1595         cap = _bfee_get_entry_cap_by_macid(adapter, macid);
1596
1597         return cap;
1598 }
1599
1600 struct beamformer_entry *rtw_bf_bfer_get_entry_by_addr(PADAPTER adapter, u8 *ra)
1601 {
1602         return _bfer_get_entry_by_addr(adapter, ra);
1603 }
1604
1605 struct beamformee_entry *rtw_bf_bfee_get_entry_by_addr(PADAPTER adapter, u8 *ra)
1606 {
1607         return _bfee_get_entry_by_addr(adapter, ra);
1608 }
1609
1610 void rtw_bf_get_ndpa_packet(PADAPTER adapter, union recv_frame *precv_frame)
1611 {
1612         RTW_DBG("+%s\n", __FUNCTION__);
1613 }
1614
1615 u32 rtw_bf_get_report_packet(PADAPTER adapter, union recv_frame *precv_frame)
1616 {
1617         u32 ret = _SUCCESS;
1618         struct beamforming_info *info;
1619         struct beamformee_entry *bfee = NULL;
1620         u8 *pframe;
1621         u32 frame_len;
1622         u8 *ta;
1623         u8 *frame_body;
1624         u8 category, action;
1625         u8 *pMIMOCtrlField, *pCSIMatrix;
1626         u8 Nc = 0, Nr = 0, CH_W = 0, Ng = 0, CodeBook = 0;
1627         u16 CSIMatrixLen = 0;
1628
1629
1630         RTW_INFO("+%s\n", __FUNCTION__);
1631
1632         info = GET_BEAMFORM_INFO(adapter);
1633         pframe = precv_frame->u.hdr.rx_data;
1634         frame_len = precv_frame->u.hdr.len;
1635
1636         /* Memory comparison to see if CSI report is the same with previous one */
1637         ta = get_addr2_ptr(pframe);
1638         bfee = _bfee_get_entry_by_addr(adapter, ta);
1639         if (!bfee)
1640                 return _FAIL;
1641
1642         frame_body = pframe + sizeof(struct rtw_ieee80211_hdr_3addr);
1643         category = frame_body[0];
1644         action = frame_body[1];
1645
1646         if ((category == RTW_WLAN_CATEGORY_VHT)
1647             && (action == RTW_WLAN_ACTION_VHT_COMPRESSED_BEAMFORMING)) {
1648                 pMIMOCtrlField = pframe + 26;
1649                 Nc = (*pMIMOCtrlField) & 0x7;
1650                 Nr = ((*pMIMOCtrlField) & 0x38) >> 3;
1651                 CH_W =  (((*pMIMOCtrlField) & 0xC0) >> 6);
1652                 Ng = (*(pMIMOCtrlField+1)) & 0x3;
1653                 CodeBook = ((*(pMIMOCtrlField+1)) & 0x4) >> 2;
1654                 /*
1655                  * 24+(1+1+3)+2
1656                  * ==> MAC header+(Category+ActionCode+MIMOControlField)+SNR(Nc=2)
1657                  */
1658                 pCSIMatrix = pMIMOCtrlField + 3 + Nc;
1659                 CSIMatrixLen = frame_len - 26 - 3 - Nc;
1660                 info->TargetCSIInfo.bVHT = _TRUE;
1661         } else if ((category == RTW_WLAN_CATEGORY_HT)
1662                    && (action == RTW_WLAN_ACTION_HT_COMPRESS_BEAMFORMING)) {
1663                 pMIMOCtrlField = pframe + 26;
1664                 Nc = (*pMIMOCtrlField) & 0x3;
1665                 Nr = ((*pMIMOCtrlField) & 0xC) >> 2;
1666                 CH_W = ((*pMIMOCtrlField) & 0x10) >> 4;
1667                 Ng = ((*pMIMOCtrlField) & 0x60) >> 5;
1668                 CodeBook = ((*(pMIMOCtrlField+1)) & 0x6) >> 1;
1669                 /*
1670                  * 24+(1+1+6)+2
1671                  * ==> MAC header+(Category+ActionCode+MIMOControlField)+SNR(Nc=2)
1672                  */
1673                 pCSIMatrix = pMIMOCtrlField + 6 + Nr;
1674                 CSIMatrixLen = frame_len  - 26 - 6 - Nr;
1675                 info->TargetCSIInfo.bVHT = _FALSE;
1676         }
1677
1678         /* Update current CSI report info */
1679         if ((_TRUE == info->bEnableSUTxBFWorkAround)
1680             && (info->TargetSUBFee == bfee)) {
1681                 if ((info->TargetCSIInfo.Nc != Nc) || (info->TargetCSIInfo.Nr != Nr) ||
1682                         (info->TargetCSIInfo.ChnlWidth != CH_W) || (info->TargetCSIInfo.Ng != Ng) ||
1683                         (info->TargetCSIInfo.CodeBook != CodeBook)) {
1684                         info->TargetCSIInfo.Nc = Nc;
1685                         info->TargetCSIInfo.Nr = Nr;
1686                         info->TargetCSIInfo.ChnlWidth = CH_W;
1687                         info->TargetCSIInfo.Ng = Ng;
1688                         info->TargetCSIInfo.CodeBook = CodeBook;
1689
1690                         rtw_bf_cmd(adapter, BEAMFORMING_CTRL_SET_CSI_REPORT, (u8*)&info->TargetCSIInfo, sizeof(struct _RT_CSI_INFO), 1);
1691                 }
1692         }
1693
1694         RTW_INFO("%s: pkt type=%d-%d, Nc=%d, Nr=%d, CH_W=%d, Ng=%d, CodeBook=%d\n",
1695                  __FUNCTION__, category, action, Nc, Nr, CH_W, Ng, CodeBook);
1696
1697         return ret;
1698 }
1699
1700 u8 rtw_bf_send_vht_gid_mgnt_packet(PADAPTER adapter, u8 *ra, u8 *gid, u8 *position)
1701 {
1702         /* General */
1703         struct xmit_priv *xmitpriv;
1704         struct mlme_priv *mlmepriv;
1705         struct xmit_frame *pmgntframe;
1706         /* MISC */
1707         struct pkt_attrib *attrib;
1708         struct rtw_ieee80211_hdr *wlanhdr;
1709         u8 *pframe, *ptr;
1710
1711
1712         xmitpriv = &adapter->xmitpriv;
1713         mlmepriv = &adapter->mlmepriv;
1714
1715         pmgntframe = alloc_mgtxmitframe(xmitpriv);
1716         if (!pmgntframe)
1717                 return _FALSE;
1718
1719         /* update attribute */
1720         attrib = &pmgntframe->attrib;
1721         update_mgntframe_attrib(adapter, attrib);
1722         attrib->rate = MGN_6M;
1723         attrib->bwmode = CHANNEL_WIDTH_20;
1724         attrib->subtype = WIFI_ACTION;
1725
1726         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
1727
1728         pframe = (u8 *)pmgntframe->buf_addr + TXDESC_OFFSET;
1729         wlanhdr = (struct rtw_ieee80211_hdr *)pframe;
1730
1731         wlanhdr->frame_ctl = 0;
1732         set_frame_sub_type(pframe, attrib->subtype);
1733         set_duration(pframe, 0);
1734         SetFragNum(pframe, 0);
1735         SetSeqNum(pframe, 0);
1736
1737         _rtw_memcpy(wlanhdr->addr1, ra, ETH_ALEN);
1738         _rtw_memcpy(wlanhdr->addr2, adapter_mac_addr(adapter), ETH_ALEN);
1739         _rtw_memcpy(wlanhdr->addr3, get_bssid(mlmepriv), ETH_ALEN);
1740
1741         pframe[24] = RTW_WLAN_CATEGORY_VHT;
1742         pframe[25] = RTW_WLAN_ACTION_VHT_GROUPID_MANAGEMENT;
1743         /* Set Membership Status Array */
1744         ptr = pframe + 26;
1745         _rtw_memcpy(ptr, gid, 8);
1746         /* Set User Position Array */
1747         ptr = pframe + 34;
1748         _rtw_memcpy(ptr, position, 16);
1749
1750         attrib->pktlen = 54;
1751         attrib->last_txcmdsz = attrib->pktlen;
1752
1753         dump_mgntframe(adapter, pmgntframe);
1754
1755         return _TRUE;
1756 }
1757
1758 /*
1759  * Description:
1760  *      On VHT GID management frame by an MU beamformee.
1761  */
1762 void rtw_bf_get_vht_gid_mgnt_packet(PADAPTER adapter, union recv_frame *precv_frame)
1763 {
1764         u8 *pframe;
1765         u8 *ta, *gid, *position;
1766
1767
1768         RTW_DBG("+%s\n", __FUNCTION__);
1769
1770         pframe = precv_frame->u.hdr.rx_data;
1771
1772         /* Get address by Addr2 */
1773         ta = get_addr2_ptr(pframe);
1774         /* Remove signaling TA */
1775         ta[0] &= 0xFE;
1776
1777         /* Membership Status Array */
1778         gid = pframe + 26;
1779         /* User Position Array */
1780         position= pframe + 34;
1781
1782         _bfer_set_entry_gid(adapter, ta, gid, position);
1783 }
1784
1785 void rtw_bf_init(PADAPTER adapter)
1786 {
1787         struct beamforming_info *info;
1788
1789
1790         info = GET_BEAMFORM_INFO(adapter);
1791         info->beamforming_cap = BEAMFORMING_CAP_NONE;
1792         info->beamforming_state = BEAMFORMING_STATE_IDLE;
1793 /*
1794         info->bfee_entry[MAX_BEAMFORMEE_ENTRY_NUM];
1795         info->bfer_entry[MAX_BEAMFORMER_ENTRY_NUM];
1796 */
1797         info->sounding_sequence = 0;
1798         info->beamformee_su_cnt = 0;
1799         info->beamformer_su_cnt = 0;
1800         info->beamformee_su_reg_maping = 0;
1801         info->beamformer_su_reg_maping = 0;
1802         info->beamformee_mu_cnt = 0;
1803         info->beamformer_mu_cnt = 0;
1804         info->beamformee_mu_reg_maping = 0;
1805         info->first_mu_bfee_index = 0xFF;
1806         info->mu_bfer_curidx = 0xFF;
1807
1808         _sounding_init(&info->sounding_info);
1809         _init_timer(&info->sounding_timer, adapter->pnetdev, _sounding_timer_handler, adapter);
1810         _init_timer(&info->sounding_timeout_timer, adapter->pnetdev, _sounding_timeout_timer_handler, adapter);
1811
1812         info->SetHalBFEnterOnDemandCnt = 0;
1813         info->SetHalBFLeaveOnDemandCnt = 0;
1814         info->SetHalSoundownOnDemandCnt = 0;
1815
1816         info->bEnableSUTxBFWorkAround = _TRUE;
1817         info->TargetSUBFee = NULL;
1818
1819         info->sounding_running = 0;
1820 }
1821
1822 void rtw_bf_cmd_hdl(PADAPTER adapter, u8 type, u8 *pbuf)
1823 {
1824         switch (type) {
1825         case BEAMFORMING_CTRL_ENTER:
1826                 _beamforming_enter(adapter, pbuf);
1827                 break;
1828
1829         case BEAMFORMING_CTRL_LEAVE:
1830                 if (pbuf == NULL)
1831                         _beamforming_reset(adapter);
1832                 else
1833                         _beamforming_leave(adapter, pbuf);
1834                 break;
1835
1836         case BEAMFORMING_CTRL_START_PERIOD:
1837                 _sounding_handler(adapter);
1838                 break;
1839
1840         case BEAMFORMING_CTRL_END_PERIOD:
1841                 _beamforming_sounding_down(adapter, *pbuf);
1842                 break;
1843
1844         case BEAMFORMING_CTRL_SET_GID_TABLE:
1845                 rtw_hal_set_hwreg(adapter, HW_VAR_SOUNDING_SET_GID_TABLE, *(void**)pbuf);
1846                 break;
1847
1848         case BEAMFORMING_CTRL_SET_CSI_REPORT:
1849                 rtw_hal_set_hwreg(adapter, HW_VAR_SOUNDING_CSI_REPORT, pbuf);
1850                 break;
1851
1852         default:
1853                 break;
1854         }
1855 }
1856
1857 u8 rtw_bf_cmd(PADAPTER adapter, s32 type, u8 *pbuf, s32 size, u8 enqueue)
1858 {
1859         struct cmd_obj *ph2c;
1860         struct drvextra_cmd_parm *pdrvextra_cmd_parm;
1861         struct cmd_priv *pcmdpriv = &adapter->cmdpriv;
1862         u8 *wk_buf;
1863         u8 res = _SUCCESS;
1864
1865
1866         if (!enqueue) {
1867                 rtw_bf_cmd_hdl(adapter, type, pbuf);
1868                 goto exit;
1869         }
1870
1871         ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
1872         if (ph2c == NULL) {
1873                 res = _FAIL;
1874                 goto exit;
1875         }
1876
1877         pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm));
1878         if (pdrvextra_cmd_parm == NULL) {
1879                 rtw_mfree((unsigned char *)ph2c, sizeof(struct cmd_obj));
1880                 res = _FAIL;
1881                 goto exit;
1882         }
1883
1884         if (pbuf != NULL) {
1885                 wk_buf = rtw_zmalloc(size);
1886                 if (wk_buf == NULL) {
1887                         rtw_mfree((u8 *)ph2c, sizeof(struct cmd_obj));
1888                         rtw_mfree((u8 *)pdrvextra_cmd_parm, sizeof(struct drvextra_cmd_parm));
1889                         res = _FAIL;
1890                         goto exit;
1891                 }
1892
1893                 _rtw_memcpy(wk_buf, pbuf, size);
1894         } else {
1895                 wk_buf = NULL;
1896                 size = 0;
1897         }
1898
1899         pdrvextra_cmd_parm->ec_id = BEAMFORMING_WK_CID;
1900         pdrvextra_cmd_parm->type = type;
1901         pdrvextra_cmd_parm->size = size;
1902         pdrvextra_cmd_parm->pbuf = wk_buf;
1903
1904         init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
1905
1906         res = rtw_enqueue_cmd(pcmdpriv, ph2c);
1907
1908 exit:
1909         return res;
1910 }
1911
1912 void rtw_bf_update_attrib(PADAPTER adapter, struct pkt_attrib *attrib, struct sta_info *sta)
1913 {
1914         if (sta) {
1915                 attrib->txbf_g_id = sta->txbf_gid;
1916                 attrib->txbf_p_aid = sta->txbf_paid;
1917         }
1918 }
1919
1920 void rtw_bf_c2h_handler(PADAPTER adapter, u8 id, u8 *buf, u8 buf_len)
1921 {
1922         switch (id) {
1923         case CMD_ID_C2H_SND_TXBF:
1924                 _c2h_snd_txbf(adapter, buf, buf_len);
1925                 break;
1926         }
1927 }
1928
1929 #define toMbps(bytes, secs)     (rtw_division64(bytes >> 17, secs))
1930 void rtw_bf_update_traffic(PADAPTER adapter)
1931 {
1932         struct beamforming_info *info;
1933         struct sounding_info *sounding;
1934         struct beamformee_entry *bfee;
1935         struct sta_info *sta;
1936         u8 bfee_cnt, sounding_idx, i;
1937         u16 tp[MAX_BEAMFORMEE_ENTRY_NUM] = {0};
1938         u8 tx_rate[MAX_BEAMFORMEE_ENTRY_NUM] = {0};
1939         u64 tx_bytes, last_bytes;
1940         u32 time, last_timestamp;
1941         u8 set_timer = _FALSE;
1942
1943
1944         info = GET_BEAMFORM_INFO(adapter);
1945         sounding = &info->sounding_info;
1946
1947         /* Check any bfee exist? */
1948         bfee_cnt = info->beamformee_su_cnt + info->beamformee_mu_cnt;
1949         if (bfee_cnt == 0)
1950                 return;
1951
1952         for (i = 0; i < MAX_BEAMFORMEE_ENTRY_NUM; i++) {
1953                 bfee = &info->bfee_entry[i];
1954                 if (_FALSE == bfee->used)
1955                         continue;
1956
1957                 sta = rtw_get_stainfo(&adapter->stapriv, bfee->mac_addr);
1958                 if (!sta) {
1959                         RTW_ERR("%s: Cann't find sta_info for " MAC_FMT "!\n", __FUNCTION__, MAC_ARG(bfee->mac_addr));
1960                         continue;
1961                 }
1962
1963                 last_timestamp = bfee->tx_timestamp;
1964                 last_bytes = bfee->tx_bytes;
1965                 bfee->tx_timestamp = rtw_get_current_time();
1966                 bfee->tx_bytes = sta->sta_stats.tx_bytes;
1967                 if (last_timestamp) {
1968                         if (bfee->tx_bytes >= last_bytes)
1969                                 tx_bytes = bfee->tx_bytes - last_bytes;
1970                         else
1971                                 tx_bytes = bfee->tx_bytes + (~last_bytes);
1972                         time = rtw_get_time_interval_ms(last_timestamp, bfee->tx_timestamp);
1973                         time = (time > 1000) ? time/1000 : 1;
1974                         tp[i] = toMbps(tx_bytes, time);
1975                         tx_rate[i] = rtw_get_current_tx_rate(adapter, bfee->mac_id);
1976                         RTW_INFO("%s: BFee idx(%d), MadId(%d), TxTP=%lld bytes (%d Mbps), txrate=%d\n",
1977                                  __FUNCTION__, i, bfee->mac_id, tx_bytes, tp[i], tx_rate[i]);
1978                 }
1979         }
1980
1981         sounding_idx = phydm_get_beamforming_sounding_info(GET_PDM_ODM(adapter), tp, MAX_BEAMFORMEE_ENTRY_NUM, tx_rate);
1982
1983         for (i = 0; i < MAX_BEAMFORMEE_ENTRY_NUM; i++) {
1984                 bfee = &info->bfee_entry[i];
1985                 if (_FALSE == bfee->used) {
1986                         if (sounding_idx & BIT(i))
1987                                 RTW_WARN("%s: bfee(%d) not in used but need sounding?!\n", __FUNCTION__, i);
1988                         continue;
1989                 }
1990
1991                 if (sounding_idx & BIT(i)) {
1992                         if (_FALSE == bfee->bApplySounding) {
1993                                 bfee->bApplySounding = _TRUE;
1994                                 bfee->SoundCnt = 0;
1995                                 set_timer = _TRUE;
1996                         }
1997                 } else {
1998                         if (_TRUE == bfee->bApplySounding) {
1999                                 bfee->bApplySounding = _FALSE;
2000                                 bfee->bDeleteSounding = _TRUE;
2001                                 bfee->SoundCnt = 0;
2002                                 set_timer = _TRUE;
2003                         }
2004                 }
2005         }
2006
2007         if (_TRUE == set_timer) {
2008                 if (SOUNDING_STATE_NONE == info->sounding_info.state) {
2009                         info->sounding_info.state = SOUNDING_STATE_INIT;
2010                         _set_timer(&info->sounding_timer, 0);
2011                 }
2012         }
2013 }
2014
2015 #else /* !RTW_BEAMFORMING_VERSION_2 */
2016
2017 #if (BEAMFORMING_SUPPORT == 0) /*for diver defined beamforming*/
2018 struct beamforming_entry        *beamforming_get_entry_by_addr(struct mlme_priv *pmlmepriv, u8 *ra, u8 *idx)
2019 {
2020         u8      i = 0;
2021         struct beamforming_info *pBeamInfo = GET_BEAMFORM_INFO(pmlmepriv);
2022
2023         for (i = 0; i < BEAMFORMING_ENTRY_NUM; i++) {
2024                 if (pBeamInfo->beamforming_entry[i].bUsed &&
2025                     (_rtw_memcmp(ra, pBeamInfo->beamforming_entry[i].mac_addr, ETH_ALEN))) {
2026                         *idx = i;
2027                         return &(pBeamInfo->beamforming_entry[i]);
2028                 }
2029         }
2030
2031         return NULL;
2032 }
2033
2034 BEAMFORMING_CAP beamforming_get_entry_beam_cap_by_mac_id(PVOID pmlmepriv , u8 mac_id)
2035 {
2036         u8      i = 0;
2037         struct beamforming_info *pBeamInfo = GET_BEAMFORM_INFO((struct mlme_priv *)pmlmepriv);
2038         BEAMFORMING_CAP         BeamformEntryCap = BEAMFORMING_CAP_NONE;
2039
2040         for (i = 0; i < BEAMFORMING_ENTRY_NUM; i++) {
2041                 if (pBeamInfo->beamforming_entry[i].bUsed &&
2042                     (mac_id == pBeamInfo->beamforming_entry[i].mac_id)) {
2043                         BeamformEntryCap =  pBeamInfo->beamforming_entry[i].beamforming_entry_cap;
2044                         i = BEAMFORMING_ENTRY_NUM;
2045                 }
2046         }
2047
2048         return BeamformEntryCap;
2049 }
2050
2051 struct beamforming_entry        *beamforming_get_free_entry(struct mlme_priv *pmlmepriv, u8 *idx)
2052 {
2053         u8      i = 0;
2054         struct beamforming_info *pBeamInfo = GET_BEAMFORM_INFO(pmlmepriv);
2055
2056         for (i = 0; i < BEAMFORMING_ENTRY_NUM; i++) {
2057                 if (pBeamInfo->beamforming_entry[i].bUsed == _FALSE) {
2058                         *idx = i;
2059                         return &(pBeamInfo->beamforming_entry[i]);
2060                 }
2061         }
2062         return NULL;
2063 }
2064
2065
2066 struct beamforming_entry        *beamforming_add_entry(PADAPTER adapter, u8 *ra, u16 aid,
2067         u16 mac_id, CHANNEL_WIDTH bw, BEAMFORMING_CAP beamfrom_cap, u8 *idx)
2068 {
2069         struct mlme_priv                        *pmlmepriv = &(adapter->mlmepriv);
2070         struct beamforming_entry        *pEntry = beamforming_get_free_entry(pmlmepriv, idx);
2071
2072         if (pEntry != NULL) {
2073                 pEntry->bUsed = _TRUE;
2074                 pEntry->aid = aid;
2075                 pEntry->mac_id = mac_id;
2076                 pEntry->sound_bw = bw;
2077                 if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
2078                         u16     BSSID = ((*(adapter_mac_addr(adapter) + 5) & 0xf0) >> 4) ^
2079                                 (*(adapter_mac_addr(adapter) + 5) & 0xf); /* BSSID[44:47] xor BSSID[40:43] */
2080                         pEntry->p_aid = (aid + BSSID * 32) & 0x1ff;             /* (dec(A) + dec(B)*32) mod 512 */
2081                         pEntry->g_id = 63;
2082                 } else if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) || check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE)) {
2083                         pEntry->p_aid = 0;
2084                         pEntry->g_id = 63;
2085                 } else {
2086                         pEntry->p_aid =  ra[5];                                         /* BSSID[39:47] */
2087                         pEntry->p_aid = (pEntry->p_aid << 1) | (ra[4] >> 7);
2088                         pEntry->g_id = 0;
2089                 }
2090                 _rtw_memcpy(pEntry->mac_addr, ra, ETH_ALEN);
2091                 pEntry->bSound = _FALSE;
2092
2093                 /* 3 TODO SW/FW sound period */
2094                 pEntry->sound_period = 200;
2095                 pEntry->beamforming_entry_cap = beamfrom_cap;
2096                 pEntry->beamforming_entry_state = BEAMFORMING_ENTRY_STATE_UNINITIALIZE;
2097
2098
2099                 pEntry->PreLogSeq = 0;  /*Modified by Jeffery @2015-04-13*/
2100                 pEntry->LogSeq = 0;             /*Modified by Jeffery @2014-10-29*/
2101                 pEntry->LogRetryCnt = 0;        /*Modified by Jeffery @2014-10-29*/
2102                 pEntry->LogSuccess = 0; /*LogSuccess is NOT needed to be accumulated, so  LogSuccessCnt->LogSuccess, 2015-04-13, Jeffery*/
2103                 pEntry->ClockResetTimes = 0;    /*Modified by Jeffery @2015-04-13*/
2104                 pEntry->LogStatusFailCnt = 0;
2105
2106                 return pEntry;
2107         } else
2108                 return NULL;
2109 }
2110
2111 BOOLEAN beamforming_remove_entry(struct mlme_priv *pmlmepriv, u8 *ra, u8 *idx)
2112 {
2113         struct beamforming_entry        *pEntry = beamforming_get_entry_by_addr(pmlmepriv, ra, idx);
2114
2115         if (pEntry != NULL) {
2116                 pEntry->bUsed = _FALSE;
2117                 pEntry->beamforming_entry_cap = BEAMFORMING_CAP_NONE;
2118                 pEntry->beamforming_entry_state = BEAMFORMING_ENTRY_STATE_UNINITIALIZE;
2119                 return _TRUE;
2120         } else
2121                 return _FALSE;
2122 }
2123
2124 /* Used for BeamformingStart_V1 */
2125 void    beamforming_dym_ndpa_rate(PADAPTER adapter)
2126 {
2127         u16     NDPARate = MGN_6M;
2128         PHAL_DATA_TYPE  pHalData = GET_HAL_DATA(adapter);
2129
2130         if (pHalData->min_undecorated_pwdb_for_dm > 30) /* link RSSI > 30% */
2131                 NDPARate = MGN_24M;
2132         else
2133                 NDPARate = MGN_6M;
2134
2135         /* BW = CHANNEL_WIDTH_20; */
2136         NDPARate = NDPARate << 8;
2137         rtw_hal_set_hwreg(adapter, HW_VAR_SOUNDING_RATE, (u8 *)&NDPARate);
2138 }
2139
2140 void beamforming_dym_period(PADAPTER Adapter)
2141 {
2142         u8      Idx;
2143         BOOLEAN bChangePeriod = _FALSE;
2144         u16     SoundPeriod_SW, SoundPeriod_FW;
2145         PHAL_DATA_TYPE  pHalData = GET_HAL_DATA(Adapter);
2146         struct dvobj_priv       *pdvobjpriv = adapter_to_dvobj(Adapter);
2147         struct beamforming_entry        *pBeamformEntry;
2148         struct beamforming_info *pBeamInfo = GET_BEAMFORM_INFO((&Adapter->mlmepriv));
2149         struct sounding_info            *pSoundInfo = &(pBeamInfo->sounding_info);
2150
2151         /* 3 TODO  per-client throughput caculation. */
2152
2153         if (pdvobjpriv->traffic_stat.cur_tx_tp + pdvobjpriv->traffic_stat.cur_rx_tp > 2) {
2154                 SoundPeriod_SW = 32 * 20;
2155                 SoundPeriod_FW = 2;
2156         } else {
2157                 SoundPeriod_SW = 32 * 2000;
2158                 SoundPeriod_FW = 200;
2159         }
2160
2161         for (Idx = 0; Idx < BEAMFORMING_ENTRY_NUM; Idx++) {
2162                 pBeamformEntry = pBeamInfo->beamforming_entry + Idx;
2163                 if (pBeamformEntry->bDefaultCSI) {
2164                         SoundPeriod_SW = 32 * 2000;
2165                         SoundPeriod_FW = 200;
2166                 }
2167
2168                 if (pBeamformEntry->beamforming_entry_cap & (BEAMFORMER_CAP_HT_EXPLICIT | BEAMFORMER_CAP_VHT_SU)) {
2169                         if (pSoundInfo->sound_mode == SOUNDING_FW_VHT_TIMER || pSoundInfo->sound_mode == SOUNDING_FW_HT_TIMER) {
2170                                 if (pBeamformEntry->sound_period != SoundPeriod_FW) {
2171                                         pBeamformEntry->sound_period = SoundPeriod_FW;
2172                                         bChangePeriod = _TRUE;  /* Only FW sounding need to send H2C packet to change sound period. */
2173                                 }
2174                         } else if (pBeamformEntry->sound_period != SoundPeriod_SW)
2175                                 pBeamformEntry->sound_period = SoundPeriod_SW;
2176                 }
2177         }
2178
2179         if (bChangePeriod)
2180                 rtw_hal_set_hwreg(Adapter, HW_VAR_SOUNDING_FW_NDPA, (u8 *)&Idx);
2181 }
2182
2183 BOOLEAN issue_ht_sw_ndpa_packet(PADAPTER Adapter, u8 *ra, CHANNEL_WIDTH bw, u8 qidx)
2184 {
2185         struct xmit_frame               *pmgntframe;
2186         struct pkt_attrib               *pattrib;
2187         struct rtw_ieee80211_hdr        *pwlanhdr;
2188         struct xmit_priv                *pxmitpriv = &(Adapter->xmitpriv);
2189         struct mlme_ext_priv    *pmlmeext = &Adapter->mlmeextpriv;
2190         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
2191         u8      ActionHdr[4] = {ACT_CAT_VENDOR, 0x00, 0xe0, 0x4c};
2192         u8      *pframe;
2193         u16     *fctrl;
2194         u16     duration = 0;
2195         u8      aSifsTime = 0;
2196         u8      NDPTxRate = 0;
2197
2198         RTW_INFO("%s: issue_ht_sw_ndpa_packet!\n", __func__);
2199
2200         NDPTxRate = MGN_MCS8;
2201         RTW_INFO("%s: NDPTxRate =%d\n", __func__, NDPTxRate);
2202         pmgntframe = alloc_mgtxmitframe(pxmitpriv);
2203
2204         if (pmgntframe == NULL)
2205                 return _FALSE;
2206
2207         /*update attribute*/
2208         pattrib = &pmgntframe->attrib;
2209         update_mgntframe_attrib(Adapter, pattrib);
2210         pattrib->qsel = QSLT_MGNT;
2211         pattrib->rate = NDPTxRate;
2212         pattrib->bwmode = bw;
2213         pattrib->order = 1;
2214         pattrib->subtype = WIFI_ACTION_NOACK;
2215
2216         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
2217
2218         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
2219
2220         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
2221
2222         fctrl = &pwlanhdr->frame_ctl;
2223         *(fctrl) = 0;
2224
2225         set_order_bit(pframe);
2226         set_frame_sub_type(pframe, WIFI_ACTION_NOACK);
2227
2228         _rtw_memcpy(pwlanhdr->addr1, ra, ETH_ALEN);
2229         _rtw_memcpy(pwlanhdr->addr2, adapter_mac_addr(Adapter), ETH_ALEN);
2230         _rtw_memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
2231
2232         if (pmlmeext->cur_wireless_mode == WIRELESS_11B)
2233                 aSifsTime = 10;
2234         else
2235                 aSifsTime = 16;
2236
2237         duration = 2 * aSifsTime + 40;
2238
2239         if (bw == CHANNEL_WIDTH_40)
2240                 duration += 87;
2241         else
2242                 duration += 180;
2243
2244         set_duration(pframe, duration);
2245
2246         /*HT control field*/
2247         SET_HT_CTRL_CSI_STEERING(pframe + 24, 3);
2248         SET_HT_CTRL_NDP_ANNOUNCEMENT(pframe + 24, 1);
2249
2250         _rtw_memcpy(pframe + 28, ActionHdr, 4);
2251
2252         pattrib->pktlen = 32;
2253
2254         pattrib->last_txcmdsz = pattrib->pktlen;
2255
2256         dump_mgntframe(Adapter, pmgntframe);
2257
2258         return _TRUE;
2259
2260
2261 }
2262 BOOLEAN issue_ht_ndpa_packet(PADAPTER Adapter, u8 *ra, CHANNEL_WIDTH bw, u8 qidx)
2263 {
2264         struct xmit_frame               *pmgntframe;
2265         struct pkt_attrib               *pattrib;
2266         struct rtw_ieee80211_hdr        *pwlanhdr;
2267         struct xmit_priv                *pxmitpriv = &(Adapter->xmitpriv);
2268         struct mlme_ext_priv    *pmlmeext = &Adapter->mlmeextpriv;
2269         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
2270         u8      ActionHdr[4] = {ACT_CAT_VENDOR, 0x00, 0xe0, 0x4c};
2271         u8      *pframe;
2272         u16     *fctrl;
2273         u16     duration = 0;
2274         u8      aSifsTime = 0;
2275
2276         pmgntframe = alloc_mgtxmitframe(pxmitpriv);
2277
2278         if (pmgntframe == NULL)
2279                 return _FALSE;
2280
2281         /*update attribute*/
2282         pattrib = &pmgntframe->attrib;
2283         update_mgntframe_attrib(Adapter, pattrib);
2284
2285         if (qidx == BCN_QUEUE_INX)
2286                 pattrib->qsel = QSLT_BEACON;
2287         pattrib->rate = MGN_MCS8;
2288         pattrib->bwmode = bw;
2289         pattrib->order = 1;
2290         pattrib->subtype = WIFI_ACTION_NOACK;
2291
2292         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
2293
2294         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
2295
2296         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
2297
2298         fctrl = &pwlanhdr->frame_ctl;
2299         *(fctrl) = 0;
2300
2301         set_order_bit(pframe);
2302         set_frame_sub_type(pframe, WIFI_ACTION_NOACK);
2303
2304         _rtw_memcpy(pwlanhdr->addr1, ra, ETH_ALEN);
2305         _rtw_memcpy(pwlanhdr->addr2, adapter_mac_addr(Adapter), ETH_ALEN);
2306         _rtw_memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
2307
2308         if (pmlmeext->cur_wireless_mode == WIRELESS_11B)
2309                 aSifsTime = 10;
2310         else
2311                 aSifsTime = 16;
2312
2313         duration = 2 * aSifsTime + 40;
2314
2315         if (bw == CHANNEL_WIDTH_40)
2316                 duration += 87;
2317         else
2318                 duration += 180;
2319
2320         set_duration(pframe, duration);
2321
2322         /* HT control field */
2323         SET_HT_CTRL_CSI_STEERING(pframe + 24, 3);
2324         SET_HT_CTRL_NDP_ANNOUNCEMENT(pframe + 24, 1);
2325
2326         _rtw_memcpy(pframe + 28, ActionHdr, 4);
2327
2328         pattrib->pktlen = 32;
2329
2330         pattrib->last_txcmdsz = pattrib->pktlen;
2331
2332         dump_mgntframe(Adapter, pmgntframe);
2333
2334         return _TRUE;
2335 }
2336
2337 BOOLEAN beamforming_send_ht_ndpa_packet(PADAPTER Adapter, u8 *ra, CHANNEL_WIDTH bw, u8 qidx)
2338 {
2339         return issue_ht_ndpa_packet(Adapter, ra, bw, qidx);
2340 }
2341 BOOLEAN issue_vht_sw_ndpa_packet(PADAPTER Adapter, u8 *ra, u16 aid, CHANNEL_WIDTH bw, u8 qidx)
2342 {
2343         struct xmit_frame               *pmgntframe;
2344         struct pkt_attrib               *pattrib;
2345         struct rtw_ieee80211_hdr        *pwlanhdr;
2346         struct xmit_priv                *pxmitpriv = &(Adapter->xmitpriv);
2347         struct mlme_ext_priv    *pmlmeext = &Adapter->mlmeextpriv;
2348         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
2349         struct mlme_priv                *pmlmepriv = &(Adapter->mlmepriv);
2350         struct beamforming_info *pBeamInfo = GET_BEAMFORM_INFO(pmlmepriv);
2351         struct rtw_ndpa_sta_info        sta_info;
2352         u8               NDPTxRate = 0;
2353
2354         u8      *pframe;
2355         u16     *fctrl;
2356         u16     duration = 0;
2357         u8      sequence = 0, aSifsTime = 0;
2358
2359         RTW_INFO("%s: issue_vht_sw_ndpa_packet!\n", __func__);
2360
2361
2362         NDPTxRate = MGN_VHT2SS_MCS0;
2363         RTW_INFO("%s: NDPTxRate =%d\n", __func__, NDPTxRate);
2364         pmgntframe = alloc_mgtxmitframe(pxmitpriv);
2365
2366         if (pmgntframe == NULL) {
2367                 RTW_INFO("%s, alloc mgnt frame fail\n", __func__);
2368                 return _FALSE;
2369         }
2370
2371         /*update attribute*/
2372         pattrib = &pmgntframe->attrib;
2373         update_mgntframe_attrib(Adapter, pattrib);
2374         pattrib->qsel = QSLT_MGNT;
2375         pattrib->rate = NDPTxRate;
2376         pattrib->bwmode = bw;
2377         pattrib->subtype = WIFI_NDPA;
2378
2379         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
2380
2381         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
2382
2383         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
2384
2385         fctrl = &pwlanhdr->frame_ctl;
2386         *(fctrl) = 0;
2387
2388         set_frame_sub_type(pframe, WIFI_NDPA);
2389
2390         _rtw_memcpy(pwlanhdr->addr1, ra, ETH_ALEN);
2391         _rtw_memcpy(pwlanhdr->addr2, adapter_mac_addr(Adapter), ETH_ALEN);
2392
2393         if (is_supported_5g(pmlmeext->cur_wireless_mode) || is_supported_ht(pmlmeext->cur_wireless_mode))
2394                 aSifsTime = 16;
2395         else
2396                 aSifsTime = 10;
2397
2398         duration = 2 * aSifsTime + 44;
2399
2400         if (bw == CHANNEL_WIDTH_80)
2401                 duration += 40;
2402         else if (bw == CHANNEL_WIDTH_40)
2403                 duration += 87;
2404         else
2405                 duration += 180;
2406
2407         set_duration(pframe, duration);
2408
2409         sequence = pBeamInfo->sounding_sequence << 2;
2410         if (pBeamInfo->sounding_sequence >= 0x3f)
2411                 pBeamInfo->sounding_sequence = 0;
2412         else
2413                 pBeamInfo->sounding_sequence++;
2414
2415         _rtw_memcpy(pframe + 16, &sequence, 1);
2416         if (((pmlmeinfo->state & 0x03) == WIFI_FW_ADHOC_STATE) || ((pmlmeinfo->state & 0x03) == WIFI_FW_AP_STATE))
2417                 aid = 0;
2418
2419         sta_info.aid = aid;
2420         sta_info.feedback_type = 0;
2421         sta_info.nc_index = 0;
2422
2423         _rtw_memcpy(pframe + 17, (u8 *)&sta_info, 2);
2424
2425         pattrib->pktlen = 19;
2426
2427         pattrib->last_txcmdsz = pattrib->pktlen;
2428
2429         dump_mgntframe(Adapter, pmgntframe);
2430
2431
2432         return _TRUE;
2433
2434 }
2435 BOOLEAN issue_vht_ndpa_packet(PADAPTER Adapter, u8 *ra, u16 aid, CHANNEL_WIDTH bw, u8 qidx)
2436 {
2437         struct xmit_frame               *pmgntframe;
2438         struct pkt_attrib               *pattrib;
2439         struct rtw_ieee80211_hdr        *pwlanhdr;
2440         struct xmit_priv                *pxmitpriv = &(Adapter->xmitpriv);
2441         struct mlme_ext_priv    *pmlmeext = &Adapter->mlmeextpriv;
2442         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
2443         struct mlme_priv                *pmlmepriv = &(Adapter->mlmepriv);
2444         struct beamforming_info *pBeamInfo = GET_BEAMFORM_INFO(pmlmepriv);
2445         struct rtw_ndpa_sta_info        sta_info;
2446         u8      *pframe;
2447         u16     *fctrl;
2448         u16     duration = 0;
2449         u8      sequence = 0, aSifsTime = 0;
2450
2451         pmgntframe = alloc_mgtxmitframe(pxmitpriv);
2452         if (pmgntframe == NULL)
2453                 return _FALSE;
2454
2455         /*update attribute*/
2456         pattrib = &pmgntframe->attrib;
2457         update_mgntframe_attrib(Adapter, pattrib);
2458
2459         if (qidx == BCN_QUEUE_INX)
2460                 pattrib->qsel = QSLT_BEACON;
2461         pattrib->rate = MGN_VHT2SS_MCS0;
2462         pattrib->bwmode = bw;
2463         pattrib->subtype = WIFI_NDPA;
2464
2465         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
2466
2467         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
2468
2469         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
2470
2471         fctrl = &pwlanhdr->frame_ctl;
2472         *(fctrl) = 0;
2473
2474         set_frame_sub_type(pframe, WIFI_NDPA);
2475
2476         _rtw_memcpy(pwlanhdr->addr1, ra, ETH_ALEN);
2477         _rtw_memcpy(pwlanhdr->addr2, adapter_mac_addr(Adapter), ETH_ALEN);
2478
2479         if (is_supported_5g(pmlmeext->cur_wireless_mode) || is_supported_ht(pmlmeext->cur_wireless_mode))
2480                 aSifsTime = 16;
2481         else
2482                 aSifsTime = 10;
2483
2484         duration = 2 * aSifsTime + 44;
2485
2486         if (bw == CHANNEL_WIDTH_80)
2487                 duration += 40;
2488         else if (bw == CHANNEL_WIDTH_40)
2489                 duration += 87;
2490         else
2491                 duration += 180;
2492
2493         set_duration(pframe, duration);
2494
2495         sequence = pBeamInfo->sounding_sequence << 2;
2496         if (pBeamInfo->sounding_sequence >= 0x3f)
2497                 pBeamInfo->sounding_sequence = 0;
2498         else
2499                 pBeamInfo->sounding_sequence++;
2500
2501         _rtw_memcpy(pframe + 16, &sequence, 1);
2502
2503         if (((pmlmeinfo->state & 0x03) == WIFI_FW_ADHOC_STATE) || ((pmlmeinfo->state & 0x03) == WIFI_FW_AP_STATE))
2504                 aid = 0;
2505
2506         sta_info.aid = aid;
2507         sta_info.feedback_type = 0;
2508         sta_info.nc_index = 0;
2509
2510         _rtw_memcpy(pframe + 17, (u8 *)&sta_info, 2);
2511
2512         pattrib->pktlen = 19;
2513
2514         pattrib->last_txcmdsz = pattrib->pktlen;
2515
2516         dump_mgntframe(Adapter, pmgntframe);
2517
2518         return _TRUE;
2519 }
2520
2521 BOOLEAN beamforming_send_vht_ndpa_packet(PADAPTER Adapter, u8 *ra, u16 aid, CHANNEL_WIDTH bw, u8 qidx)
2522 {
2523         return issue_vht_ndpa_packet(Adapter, ra, aid, bw, qidx);
2524 }
2525
2526 BOOLEAN beamfomring_bSounding(struct beamforming_info *pBeamInfo)
2527 {
2528         BOOLEAN         bSounding = _FALSE;
2529
2530         if ((beamforming_get_beamform_cap(pBeamInfo) & BEAMFORMER_CAP) == 0)
2531                 bSounding = _FALSE;
2532         else
2533                 bSounding = _TRUE;
2534
2535         return bSounding;
2536 }
2537
2538 u8      beamforming_sounding_idx(struct beamforming_info *pBeamInfo)
2539 {
2540         u8      idx = 0;
2541         u8      i;
2542
2543         for (i = 0; i < BEAMFORMING_ENTRY_NUM; i++) {
2544                 if (pBeamInfo->beamforming_entry[i].bUsed &&
2545                     (_FALSE == pBeamInfo->beamforming_entry[i].bSound)) {
2546                         idx = i;
2547                         break;
2548                 }
2549         }
2550
2551         return idx;
2552 }
2553
2554 SOUNDING_MODE   beamforming_sounding_mode(struct beamforming_info *pBeamInfo, u8 idx)
2555 {
2556         struct beamforming_entry        BeamEntry = pBeamInfo->beamforming_entry[idx];
2557         SOUNDING_MODE   mode;
2558
2559         if (BeamEntry.beamforming_entry_cap & BEAMFORMER_CAP_VHT_SU)
2560                 mode = SOUNDING_FW_VHT_TIMER;
2561         else if (BeamEntry.beamforming_entry_cap & BEAMFORMER_CAP_HT_EXPLICIT)
2562                 mode = SOUNDING_FW_HT_TIMER;
2563         else
2564                 mode = SOUNDING_STOP_All_TIMER;
2565
2566         return mode;
2567 }
2568
2569 u16     beamforming_sounding_time(struct beamforming_info *pBeamInfo, SOUNDING_MODE mode, u8 idx)
2570 {
2571         u16                                             sounding_time = 0xffff;
2572         struct beamforming_entry        BeamEntry = pBeamInfo->beamforming_entry[idx];
2573
2574         sounding_time = BeamEntry.sound_period;
2575
2576         return sounding_time;
2577 }
2578
2579 CHANNEL_WIDTH   beamforming_sounding_bw(struct beamforming_info *pBeamInfo, SOUNDING_MODE mode, u8 idx)
2580 {
2581         CHANNEL_WIDTH                           sounding_bw = CHANNEL_WIDTH_20;
2582         struct beamforming_entry                BeamEntry = pBeamInfo->beamforming_entry[idx];
2583
2584         sounding_bw = BeamEntry.sound_bw;
2585
2586         return sounding_bw;
2587 }
2588
2589 BOOLEAN beamforming_select_beam_entry(struct beamforming_info *pBeamInfo)
2590 {
2591         struct sounding_info            *pSoundInfo = &(pBeamInfo->sounding_info);
2592
2593         pSoundInfo->sound_idx = beamforming_sounding_idx(pBeamInfo);
2594
2595         if (pSoundInfo->sound_idx < BEAMFORMING_ENTRY_NUM)
2596                 pSoundInfo->sound_mode = beamforming_sounding_mode(pBeamInfo, pSoundInfo->sound_idx);
2597         else
2598                 pSoundInfo->sound_mode = SOUNDING_STOP_All_TIMER;
2599
2600         if (SOUNDING_STOP_All_TIMER == pSoundInfo->sound_mode)
2601                 return _FALSE;
2602         else {
2603                 pSoundInfo->sound_bw = beamforming_sounding_bw(pBeamInfo, pSoundInfo->sound_mode, pSoundInfo->sound_idx);
2604                 pSoundInfo->sound_period = beamforming_sounding_time(pBeamInfo, pSoundInfo->sound_mode, pSoundInfo->sound_idx);
2605                 return _TRUE;
2606         }
2607 }
2608
2609 BOOLEAN beamforming_start_fw(PADAPTER adapter, u8 idx)
2610 {
2611         u8                                              *RA = NULL;
2612         struct beamforming_entry        *pEntry;
2613         BOOLEAN                                 ret = _TRUE;
2614         struct mlme_priv                        *pmlmepriv = &(adapter->mlmepriv);
2615         struct beamforming_info *pBeamInfo = GET_BEAMFORM_INFO(pmlmepriv);
2616
2617         pEntry = &(pBeamInfo->beamforming_entry[idx]);
2618         if (pEntry->bUsed == _FALSE) {
2619                 RTW_INFO("Skip Beamforming, no entry for Idx =%d\n", idx);
2620                 return _FALSE;
2621         }
2622
2623         pEntry->beamforming_entry_state = BEAMFORMING_ENTRY_STATE_PROGRESSING;
2624         pEntry->bSound = _TRUE;
2625         rtw_hal_set_hwreg(adapter, HW_VAR_SOUNDING_FW_NDPA, (u8 *)&idx);
2626
2627         return _TRUE;
2628 }
2629
2630 void    beamforming_end_fw(PADAPTER adapter)
2631 {
2632         u8      idx = 0;
2633
2634         rtw_hal_set_hwreg(adapter, HW_VAR_SOUNDING_FW_NDPA, (u8 *)&idx);
2635
2636         RTW_INFO("%s\n", __FUNCTION__);
2637 }
2638
2639 BOOLEAN beamforming_start_period(PADAPTER adapter)
2640 {
2641         BOOLEAN ret = _TRUE;
2642         struct mlme_priv                        *pmlmepriv = &(adapter->mlmepriv);
2643         struct beamforming_info *pBeamInfo = GET_BEAMFORM_INFO(pmlmepriv);
2644         struct sounding_info            *pSoundInfo = &(pBeamInfo->sounding_info);
2645
2646         beamforming_dym_ndpa_rate(adapter);
2647
2648         beamforming_select_beam_entry(pBeamInfo);
2649
2650         if (pSoundInfo->sound_mode == SOUNDING_FW_VHT_TIMER || pSoundInfo->sound_mode == SOUNDING_FW_HT_TIMER)
2651                 ret = beamforming_start_fw(adapter, pSoundInfo->sound_idx);
2652         else
2653                 ret = _FALSE;
2654
2655         RTW_INFO("%s Idx %d Mode %d BW %d Period %d\n", __FUNCTION__,
2656                 pSoundInfo->sound_idx, pSoundInfo->sound_mode, pSoundInfo->sound_bw, pSoundInfo->sound_period);
2657
2658         return ret;
2659 }
2660
2661 void    beamforming_end_period(PADAPTER adapter)
2662 {
2663         u8                                              idx = 0;
2664         struct beamforming_entry        *pBeamformEntry;
2665         struct mlme_priv                        *pmlmepriv = &(adapter->mlmepriv);
2666         struct beamforming_info *pBeamInfo = GET_BEAMFORM_INFO(pmlmepriv);
2667         struct sounding_info            *pSoundInfo = &(pBeamInfo->sounding_info);
2668
2669
2670         if (pSoundInfo->sound_mode == SOUNDING_FW_VHT_TIMER || pSoundInfo->sound_mode == SOUNDING_FW_HT_TIMER)
2671                 beamforming_end_fw(adapter);
2672 }
2673
2674 void    beamforming_notify(PADAPTER adapter)
2675 {
2676         BOOLEAN         bSounding = _FALSE;
2677         struct beamforming_info *pBeamInfo = GET_BEAMFORM_INFO(&(adapter->mlmepriv));
2678
2679         bSounding = beamfomring_bSounding(pBeamInfo);
2680
2681         if (pBeamInfo->beamforming_state == BEAMFORMING_STATE_IDLE) {
2682                 if (bSounding) {
2683                         if (beamforming_start_period(adapter) == _TRUE)
2684                                 pBeamInfo->beamforming_state = BEAMFORMING_STATE_START;
2685                 }
2686         } else if (pBeamInfo->beamforming_state == BEAMFORMING_STATE_START) {
2687                 if (bSounding) {
2688                         if (beamforming_start_period(adapter) == _FALSE)
2689                                 pBeamInfo->beamforming_state = BEAMFORMING_STATE_END;
2690                 } else {
2691                         beamforming_end_period(adapter);
2692                         pBeamInfo->beamforming_state = BEAMFORMING_STATE_END;
2693                 }
2694         } else if (pBeamInfo->beamforming_state == BEAMFORMING_STATE_END) {
2695                 if (bSounding) {
2696                         if (beamforming_start_period(adapter) == _TRUE)
2697                                 pBeamInfo->beamforming_state = BEAMFORMING_STATE_START;
2698                 }
2699         } else
2700                 RTW_INFO("%s BeamformState %d\n", __FUNCTION__, pBeamInfo->beamforming_state);
2701
2702         RTW_INFO("%s BeamformState %d bSounding %d\n", __FUNCTION__, pBeamInfo->beamforming_state, bSounding);
2703 }
2704
2705 BOOLEAN beamforming_init_entry(PADAPTER adapter, struct sta_info *psta, u8 *idx)
2706 {
2707         struct mlme_priv        *pmlmepriv = &(adapter->mlmepriv);
2708         struct ht_priv          *phtpriv = &(pmlmepriv->htpriv);
2709 #ifdef CONFIG_80211AC_VHT
2710         struct vht_priv         *pvhtpriv = &(pmlmepriv->vhtpriv);
2711 #endif
2712         struct mlme_ext_priv    *pmlmeext = &(adapter->mlmeextpriv);
2713         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
2714         struct beamforming_entry        *pBeamformEntry = NULL;
2715         u8      *ra;
2716         u16     aid, mac_id;
2717         u8      wireless_mode;
2718         CHANNEL_WIDTH   bw = CHANNEL_WIDTH_20;
2719         BEAMFORMING_CAP beamform_cap = BEAMFORMING_CAP_NONE;
2720
2721         /* The current setting does not support Beaforming */
2722         if (0 == phtpriv->beamform_cap
2723 #ifdef CONFIG_80211AC_VHT
2724             && 0 == pvhtpriv->beamform_cap
2725 #endif
2726            ) {
2727                 RTW_INFO("The configuration disabled Beamforming! Skip...\n");
2728                 return _FALSE;
2729         }
2730
2731         aid = psta->aid;
2732         ra = psta->hwaddr;
2733         mac_id = psta->mac_id;
2734         wireless_mode = psta->wireless_mode;
2735         bw = psta->bw_mode;
2736
2737         if (is_supported_ht(wireless_mode) || is_supported_vht(wireless_mode)) {
2738                 /* 3 */ /* HT */
2739                 u8      cur_beamform;
2740
2741                 cur_beamform = psta->htpriv.beamform_cap;
2742
2743                 /* We are Beamformee because the STA is Beamformer */
2744                 if (TEST_FLAG(cur_beamform, BEAMFORMING_HT_BEAMFORMER_ENABLE))
2745                         beamform_cap = (BEAMFORMING_CAP)(beamform_cap | BEAMFORMEE_CAP_HT_EXPLICIT);
2746
2747                 /* We are Beamformer because the STA is Beamformee */
2748                 if (TEST_FLAG(cur_beamform, BEAMFORMING_HT_BEAMFORMEE_ENABLE))
2749                         beamform_cap = (BEAMFORMING_CAP)(beamform_cap | BEAMFORMER_CAP_HT_EXPLICIT);
2750 #ifdef CONFIG_80211AC_VHT
2751                 if (is_supported_vht(wireless_mode)) {
2752                         /* 3 */ /* VHT */
2753                         cur_beamform = psta->vhtpriv.beamform_cap;
2754
2755                         /* We are Beamformee because the STA is Beamformer */
2756                         if (TEST_FLAG(cur_beamform, BEAMFORMING_VHT_BEAMFORMER_ENABLE))
2757                                 beamform_cap = (BEAMFORMING_CAP)(beamform_cap | BEAMFORMEE_CAP_VHT_SU);
2758                         /* We are Beamformer because the STA is Beamformee */
2759                         if (TEST_FLAG(cur_beamform, BEAMFORMING_VHT_BEAMFORMEE_ENABLE))
2760                                 beamform_cap = (BEAMFORMING_CAP)(beamform_cap | BEAMFORMER_CAP_VHT_SU);
2761                 }
2762 #endif /* CONFIG_80211AC_VHT */
2763
2764                 if (beamform_cap == BEAMFORMING_CAP_NONE)
2765                         return _FALSE;
2766
2767                 RTW_INFO("Beamforming Config Capability = 0x%02X\n", beamform_cap);
2768
2769                 pBeamformEntry = beamforming_get_entry_by_addr(pmlmepriv, ra, idx);
2770                 if (pBeamformEntry == NULL) {
2771                         pBeamformEntry = beamforming_add_entry(adapter, ra, aid, mac_id, bw, beamform_cap, idx);
2772                         if (pBeamformEntry == NULL)
2773                                 return _FALSE;
2774                         else
2775                                 pBeamformEntry->beamforming_entry_state = BEAMFORMING_ENTRY_STATE_INITIALIZEING;
2776                 } else {
2777                         /* Entry has been created. If entry is initialing or progressing then errors occur. */
2778                         if (pBeamformEntry->beamforming_entry_state != BEAMFORMING_ENTRY_STATE_INITIALIZED &&
2779                             pBeamformEntry->beamforming_entry_state != BEAMFORMING_ENTRY_STATE_PROGRESSED) {
2780                                 RTW_INFO("Error State of Beamforming");
2781                                 return _FALSE;
2782                         } else
2783                                 pBeamformEntry->beamforming_entry_state = BEAMFORMING_ENTRY_STATE_INITIALIZEING;
2784                 }
2785
2786                 pBeamformEntry->beamforming_entry_state = BEAMFORMING_ENTRY_STATE_INITIALIZED;
2787                 psta->txbf_paid = pBeamformEntry->p_aid;
2788                 psta->txbf_gid = pBeamformEntry->g_id;
2789
2790                 RTW_INFO("%s Idx %d\n", __FUNCTION__, *idx);
2791         } else
2792                 return _FALSE;
2793
2794         return _SUCCESS;
2795 }
2796
2797 void    beamforming_deinit_entry(PADAPTER adapter, u8 *ra)
2798 {
2799         u8      idx = 0;
2800         struct mlme_priv *pmlmepriv = &(adapter->mlmepriv);
2801
2802         if (beamforming_remove_entry(pmlmepriv, ra, &idx) == _TRUE)
2803                 rtw_hal_set_hwreg(adapter, HW_VAR_SOUNDING_LEAVE, (u8 *)&idx);
2804
2805         RTW_INFO("%s Idx %d\n", __FUNCTION__, idx);
2806 }
2807
2808 void    beamforming_reset(PADAPTER adapter)
2809 {
2810         u8      idx = 0;
2811         struct mlme_priv                        *pmlmepriv = &(adapter->mlmepriv);
2812         struct beamforming_info *pBeamInfo = GET_BEAMFORM_INFO(pmlmepriv);
2813
2814         for (idx = 0; idx < BEAMFORMING_ENTRY_NUM; idx++) {
2815                 if (pBeamInfo->beamforming_entry[idx].bUsed == _TRUE) {
2816                         pBeamInfo->beamforming_entry[idx].bUsed = _FALSE;
2817                         pBeamInfo->beamforming_entry[idx].beamforming_entry_cap = BEAMFORMING_CAP_NONE;
2818                         pBeamInfo->beamforming_entry[idx].beamforming_entry_state = BEAMFORMING_ENTRY_STATE_UNINITIALIZE;
2819                         rtw_hal_set_hwreg(adapter, HW_VAR_SOUNDING_LEAVE, (u8 *)&idx);
2820                 }
2821         }
2822
2823         RTW_INFO("%s\n", __FUNCTION__);
2824 }
2825
2826 void beamforming_sounding_fail(PADAPTER Adapter)
2827 {
2828         struct mlme_priv                        *pmlmepriv = &(Adapter->mlmepriv);
2829         struct beamforming_info *pBeamInfo = GET_BEAMFORM_INFO(pmlmepriv);
2830         struct beamforming_entry        *pEntry = &(pBeamInfo->beamforming_entry[pBeamInfo->beamforming_cur_idx]);
2831
2832         pEntry->bSound = _FALSE;
2833         rtw_hal_set_hwreg(Adapter, HW_VAR_SOUNDING_FW_NDPA, (u8 *)&pBeamInfo->beamforming_cur_idx);
2834         beamforming_deinit_entry(Adapter, pEntry->mac_addr);
2835 }
2836
2837 void    beamforming_check_sounding_success(PADAPTER Adapter, BOOLEAN status)
2838 {
2839         struct mlme_priv                        *pmlmepriv = &(Adapter->mlmepriv);
2840         struct beamforming_info *pBeamInfo = GET_BEAMFORM_INFO(pmlmepriv);
2841         struct beamforming_entry        *pEntry = &(pBeamInfo->beamforming_entry[pBeamInfo->beamforming_cur_idx]);
2842
2843         if (status == 1)
2844                 pEntry->LogStatusFailCnt = 0;
2845         else {
2846                 pEntry->LogStatusFailCnt++;
2847                 RTW_INFO("%s LogStatusFailCnt %d\n", __FUNCTION__, pEntry->LogStatusFailCnt);
2848         }
2849         if (pEntry->LogStatusFailCnt > 20) {
2850                 RTW_INFO("%s LogStatusFailCnt > 20, Stop SOUNDING\n", __FUNCTION__);
2851                 /* pEntry->bSound = _FALSE; */
2852                 /* rtw_hal_set_hwreg(Adapter, HW_VAR_SOUNDING_FW_NDPA, (u8 *)&pBeamInfo->beamforming_cur_idx); */
2853                 /* beamforming_deinit_entry(Adapter, pEntry->mac_addr); */
2854                 beamforming_wk_cmd(Adapter, BEAMFORMING_CTRL_SOUNDING_FAIL, NULL, 0, 1);
2855         }
2856 }
2857
2858 void    beamforming_enter(PADAPTER adapter, PVOID psta)
2859 {
2860         u8      idx = 0xff;
2861
2862         if (beamforming_init_entry(adapter, (struct sta_info *)psta, &idx))
2863                 rtw_hal_set_hwreg(adapter, HW_VAR_SOUNDING_ENTER, (u8 *)&idx);
2864
2865         /* RTW_INFO("%s Idx %d\n", __FUNCTION__, idx); */
2866 }
2867
2868 void    beamforming_leave(PADAPTER adapter, u8 *ra)
2869 {
2870         if (ra == NULL)
2871                 beamforming_reset(adapter);
2872         else
2873                 beamforming_deinit_entry(adapter, ra);
2874
2875         beamforming_notify(adapter);
2876 }
2877
2878 BEAMFORMING_CAP beamforming_get_beamform_cap(struct beamforming_info    *pBeamInfo)
2879 {
2880         u8      i;
2881         BOOLEAN                         bSelfBeamformer = _FALSE;
2882         BOOLEAN                         bSelfBeamformee = _FALSE;
2883         struct beamforming_entry        beamforming_entry;
2884         BEAMFORMING_CAP         beamform_cap = BEAMFORMING_CAP_NONE;
2885
2886         for (i = 0; i < BEAMFORMING_ENTRY_NUM; i++) {
2887                 beamforming_entry = pBeamInfo->beamforming_entry[i];
2888
2889                 if (beamforming_entry.bUsed) {
2890                         if ((beamforming_entry.beamforming_entry_cap & BEAMFORMEE_CAP_VHT_SU) ||
2891                             (beamforming_entry.beamforming_entry_cap & BEAMFORMEE_CAP_HT_EXPLICIT))
2892                                 bSelfBeamformee = _TRUE;
2893                         if ((beamforming_entry.beamforming_entry_cap & BEAMFORMER_CAP_VHT_SU) ||
2894                             (beamforming_entry.beamforming_entry_cap & BEAMFORMER_CAP_HT_EXPLICIT))
2895                                 bSelfBeamformer = _TRUE;
2896                 }
2897
2898                 if (bSelfBeamformer && bSelfBeamformee)
2899                         i = BEAMFORMING_ENTRY_NUM;
2900         }
2901
2902         if (bSelfBeamformer)
2903                 beamform_cap |= BEAMFORMER_CAP;
2904         if (bSelfBeamformee)
2905                 beamform_cap |= BEAMFORMEE_CAP;
2906
2907         return beamform_cap;
2908 }
2909
2910 void    beamforming_watchdog(PADAPTER Adapter)
2911 {
2912         struct beamforming_info *pBeamInfo = GET_BEAMFORM_INFO((&(Adapter->mlmepriv)));
2913
2914         if (pBeamInfo->beamforming_state != BEAMFORMING_STATE_START)
2915                 return;
2916
2917         beamforming_dym_period(Adapter);
2918         beamforming_dym_ndpa_rate(Adapter);
2919 }
2920 #endif/* #if (BEAMFORMING_SUPPORT ==0) - for diver defined beamforming*/
2921
2922 u32     rtw_beamforming_get_report_frame(PADAPTER        Adapter, union recv_frame *precv_frame)
2923 {
2924         u32     ret = _SUCCESS;
2925 #if (BEAMFORMING_SUPPORT == 1)
2926         PHAL_DATA_TYPE  pHalData = GET_HAL_DATA(Adapter);
2927         struct PHY_DM_STRUCT            *pDM_Odm = &(pHalData->odmpriv);
2928
2929         ret = beamforming_get_report_frame(pDM_Odm, precv_frame);
2930
2931 #else /*(BEAMFORMING_SUPPORT == 0)- for drv beamfoming*/
2932         struct beamforming_entry        *pBeamformEntry = NULL;
2933         struct mlme_priv                        *pmlmepriv = &(Adapter->mlmepriv);
2934         u8      *pframe = precv_frame->u.hdr.rx_data;
2935         u32     frame_len = precv_frame->u.hdr.len;
2936         u8      *ta;
2937         u8      idx, offset;
2938
2939         /*RTW_INFO("rtw_beamforming_get_report_frame\n");*/
2940
2941         /*Memory comparison to see if CSI report is the same with previous one*/
2942         ta = get_addr2_ptr(pframe);
2943         pBeamformEntry = beamforming_get_entry_by_addr(pmlmepriv, ta, &idx);
2944         if (pBeamformEntry->beamforming_entry_cap & BEAMFORMER_CAP_VHT_SU)
2945                 offset = 31;    /*24+(1+1+3)+2  MAC header+(Category+ActionCode+MIMOControlField)+SNR(Nc=2)*/
2946         else if (pBeamformEntry->beamforming_entry_cap & BEAMFORMER_CAP_HT_EXPLICIT)
2947                 offset = 34;    /*24+(1+1+6)+2  MAC header+(Category+ActionCode+MIMOControlField)+SNR(Nc=2)*/
2948         else
2949                 return ret;
2950
2951         /*RTW_INFO("%s MacId %d offset=%d\n", __FUNCTION__, pBeamformEntry->mac_id, offset);*/
2952
2953         if (_rtw_memcmp(pBeamformEntry->PreCsiReport + offset, pframe + offset, frame_len - offset) == _FALSE)
2954                 pBeamformEntry->DefaultCsiCnt = 0;
2955         else
2956                 pBeamformEntry->DefaultCsiCnt++;
2957
2958         _rtw_memcpy(&pBeamformEntry->PreCsiReport, pframe, frame_len);
2959
2960         pBeamformEntry->bDefaultCSI = _FALSE;
2961
2962         if (pBeamformEntry->DefaultCsiCnt > 20)
2963                 pBeamformEntry->bDefaultCSI = _TRUE;
2964         else
2965                 pBeamformEntry->bDefaultCSI = _FALSE;
2966 #endif
2967         return ret;
2968 }
2969
2970 void    rtw_beamforming_get_ndpa_frame(PADAPTER  Adapter, union recv_frame *precv_frame)
2971 {
2972 #if (BEAMFORMING_SUPPORT == 1)
2973         PHAL_DATA_TYPE  pHalData = GET_HAL_DATA(Adapter);
2974         struct PHY_DM_STRUCT            *pDM_Odm = &(pHalData->odmpriv);
2975
2976         beamforming_get_ndpa_frame(pDM_Odm, precv_frame);
2977
2978 #else /*(BEAMFORMING_SUPPORT == 0)- for drv beamfoming*/
2979         u8      *ta;
2980         u8      idx, Sequence;
2981         u8      *pframe = precv_frame->u.hdr.rx_data;
2982         struct mlme_priv                        *pmlmepriv = &(Adapter->mlmepriv);
2983         struct beamforming_entry        *pBeamformEntry = NULL;
2984
2985         /*RTW_INFO("rtw_beamforming_get_ndpa_frame\n");*/
2986
2987         if (IS_HARDWARE_TYPE_8812(Adapter) == _FALSE)
2988                 return;
2989         else if (get_frame_sub_type(pframe) != WIFI_NDPA)
2990                 return;
2991
2992         ta = get_addr2_ptr(pframe);
2993         /*Remove signaling TA. */
2994         ta[0] = ta[0] & 0xFE;
2995
2996         pBeamformEntry = beamforming_get_entry_by_addr(pmlmepriv, ta, &idx);
2997
2998         if (pBeamformEntry == NULL)
2999                 return;
3000         else if (!(pBeamformEntry->beamforming_entry_cap & BEAMFORMEE_CAP_VHT_SU))
3001                 return;
3002         /*LogSuccess: As long as 8812A receive NDPA and feedback CSI succeed once, clock reset is NO LONGER needed !2015-04-10, Jeffery*/
3003         /*ClockResetTimes: While BFer entry always doesn't receive our CSI, clock will reset again and again.So ClockResetTimes is limited to 5 times.2015-04-13, Jeffery*/
3004         else if ((pBeamformEntry->LogSuccess == 1) || (pBeamformEntry->ClockResetTimes == 5)) {
3005                 RTW_INFO("[%s] LogSeq=%d, PreLogSeq=%d\n", __func__, pBeamformEntry->LogSeq, pBeamformEntry->PreLogSeq);
3006                 return;
3007         }
3008
3009         Sequence = (pframe[16]) >> 2;
3010         RTW_INFO("[%s] Start, Sequence=%d, LogSeq=%d, PreLogSeq=%d, LogRetryCnt=%d, ClockResetTimes=%d, LogSuccess=%d\n",
3011                 __func__, Sequence, pBeamformEntry->LogSeq, pBeamformEntry->PreLogSeq, pBeamformEntry->LogRetryCnt, pBeamformEntry->ClockResetTimes, pBeamformEntry->LogSuccess);
3012
3013         if ((pBeamformEntry->LogSeq != 0) && (pBeamformEntry->PreLogSeq != 0)) {
3014                 /*Success condition*/
3015                 if ((pBeamformEntry->LogSeq != Sequence) && (pBeamformEntry->PreLogSeq != pBeamformEntry->LogSeq)) {
3016                         /* break option for clcok reset, 2015-03-30, Jeffery */
3017                         pBeamformEntry->LogRetryCnt = 0;
3018                         /*As long as 8812A receive NDPA and feedback CSI succeed once, clock reset is no longer needed.*/
3019                         /*That is, LogSuccess is NOT needed to be reset to zero, 2015-04-13, Jeffery*/
3020                         pBeamformEntry->LogSuccess = 1;
3021
3022                 } else {/*Fail condition*/
3023
3024                         if (pBeamformEntry->LogRetryCnt == 5) {
3025                                 pBeamformEntry->ClockResetTimes++;
3026                                 pBeamformEntry->LogRetryCnt = 0;
3027
3028                                 RTW_INFO("[%s] Clock Reset!!! ClockResetTimes=%d\n",  __func__, pBeamformEntry->ClockResetTimes);
3029                                 beamforming_wk_cmd(Adapter, BEAMFORMING_CTRL_SOUNDING_CLK, NULL, 0, 1);
3030
3031                         } else
3032                                 pBeamformEntry->LogRetryCnt++;
3033                 }
3034         }
3035
3036         /*Update LogSeq & PreLogSeq*/
3037         pBeamformEntry->PreLogSeq = pBeamformEntry->LogSeq;
3038         pBeamformEntry->LogSeq = Sequence;
3039
3040 #endif
3041
3042 }
3043
3044
3045
3046
3047 void    beamforming_wk_hdl(_adapter *padapter, u8 type, u8 *pbuf)
3048 {
3049         PHAL_DATA_TYPE  pHalData = GET_HAL_DATA(padapter);
3050         struct PHY_DM_STRUCT            *pDM_Odm = &(pHalData->odmpriv);
3051
3052 #if (BEAMFORMING_SUPPORT == 1) /*(BEAMFORMING_SUPPORT == 1)- for PHYDM beamfoming*/
3053         switch (type) {
3054         case BEAMFORMING_CTRL_ENTER: {
3055                 struct sta_info *psta = (PVOID)pbuf;
3056                 u16                     staIdx = psta->mac_id;
3057
3058                 beamforming_enter(pDM_Odm, staIdx);
3059                 break;
3060         }
3061         case BEAMFORMING_CTRL_LEAVE:
3062                 beamforming_leave(pDM_Odm, pbuf);
3063                 break;
3064         default:
3065                 break;
3066
3067         }
3068 #else /*(BEAMFORMING_SUPPORT == 0)- for drv beamfoming*/
3069         switch (type) {
3070         case BEAMFORMING_CTRL_ENTER:
3071                 beamforming_enter(padapter, (PVOID)pbuf);
3072                 break;
3073
3074         case BEAMFORMING_CTRL_LEAVE:
3075                 beamforming_leave(padapter, pbuf);
3076                 break;
3077
3078         case BEAMFORMING_CTRL_SOUNDING_FAIL:
3079                 beamforming_sounding_fail(padapter);
3080                 break;
3081
3082         case BEAMFORMING_CTRL_SOUNDING_CLK:
3083                 rtw_hal_set_hwreg(padapter, HW_VAR_SOUNDING_CLK, NULL);
3084                 break;
3085
3086         default:
3087                 break;
3088         }
3089 #endif
3090 }
3091
3092 u8      beamforming_wk_cmd(_adapter *padapter, s32 type, u8 *pbuf, s32 size, u8 enqueue)
3093 {
3094         struct cmd_obj  *ph2c;
3095         struct drvextra_cmd_parm        *pdrvextra_cmd_parm;
3096         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
3097         u8      res = _SUCCESS;
3098
3099
3100         if (enqueue) {
3101                 u8      *wk_buf;
3102
3103                 ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
3104                 if (ph2c == NULL) {
3105                         res = _FAIL;
3106                         goto exit;
3107                 }
3108
3109                 pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm));
3110                 if (pdrvextra_cmd_parm == NULL) {
3111                         rtw_mfree((unsigned char *)ph2c, sizeof(struct cmd_obj));
3112                         res = _FAIL;
3113                         goto exit;
3114                 }
3115
3116                 if (pbuf != NULL) {
3117                         wk_buf = rtw_zmalloc(size);
3118                         if (wk_buf == NULL) {
3119                                 rtw_mfree((u8 *)ph2c, sizeof(struct cmd_obj));
3120                                 rtw_mfree((u8 *)pdrvextra_cmd_parm, sizeof(struct drvextra_cmd_parm));
3121                                 res = _FAIL;
3122                                 goto exit;
3123                         }
3124
3125                         _rtw_memcpy(wk_buf, pbuf, size);
3126                 } else {
3127                         wk_buf = NULL;
3128                         size = 0;
3129                 }
3130
3131                 pdrvextra_cmd_parm->ec_id = BEAMFORMING_WK_CID;
3132                 pdrvextra_cmd_parm->type = type;
3133                 pdrvextra_cmd_parm->size = size;
3134                 pdrvextra_cmd_parm->pbuf = wk_buf;
3135
3136                 init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
3137
3138                 res = rtw_enqueue_cmd(pcmdpriv, ph2c);
3139         } else
3140                 beamforming_wk_hdl(padapter, type, pbuf);
3141
3142 exit:
3143
3144
3145         return res;
3146 }
3147
3148 void update_attrib_txbf_info(_adapter *padapter, struct pkt_attrib *pattrib, struct sta_info *psta)
3149 {
3150         if (psta) {
3151                 pattrib->txbf_g_id = psta->txbf_gid;
3152                 pattrib->txbf_p_aid = psta->txbf_paid;
3153         }
3154 }
3155 #endif /* !RTW_BEAMFORMING_VERSION_2 */
3156
3157 #endif /* CONFIG_BEAMFORMING */