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

   1 /******************************************************************************\r
   2  *\r
   3  * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.\r
   4  *                                        \r
   5  * This program is free software; you can redistribute it and/or modify it\r
   6  * under the terms of version 2 of the GNU General Public License as\r
   7  * published by the Free Software Foundation.\r
   8  *\r
   9  * This program is distributed in the hope that it will be useful, but WITHOUT\r
  10  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or\r
  11  * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for\r
  12  * more details.\r
  13  *\r
  14  * You should have received a copy of the GNU General Public License along with\r
  15  * this program; if not, write to the Free Software Foundation, Inc.,\r
  16  * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA\r
  17  *\r
  18  *\r
  19  ******************************************************************************/\r
  20 #define _RTW_BEAMFORMING_C_\r
  21 \r
  22 #include <drv_types.h>\r
  23 #include <hal_data.h>\r
  24 \r
  25 #ifdef CONFIG_BEAMFORMING\r
  26 \r
  27 #if (BEAMFORMING_SUPPORT == 0) /*for diver defined beamforming*/\r
  28 struct beamforming_entry        *beamforming_get_entry_by_addr(struct mlme_priv *pmlmepriv, u8* ra,u8* idx)\r
  29 {\r
  30         u8      i = 0;\r
  31         struct beamforming_info *pBeamInfo = GET_BEAMFORM_INFO(pmlmepriv);\r
  32         \r
  33         for(i = 0; i < BEAMFORMING_ENTRY_NUM; i++)\r
  34         {\r
  35                 if(     pBeamInfo->beamforming_entry[i].bUsed && \r
  36                         (_rtw_memcmp(ra,pBeamInfo->beamforming_entry[i].mac_addr, ETH_ALEN)))\r
  37                 {\r
  38                         *idx = i;\r
  39                         return &(pBeamInfo->beamforming_entry[i]);\r
  40                 }\r
  41         }\r
  42 \r
  43         return NULL;\r
  44 }\r
  45 \r
  46 BEAMFORMING_CAP beamforming_get_entry_beam_cap_by_mac_id(PVOID pmlmepriv ,u8 mac_id)\r
  47 {\r
  48         u8      i = 0;\r
  49         struct beamforming_info *pBeamInfo = GET_BEAMFORM_INFO((struct mlme_priv *)pmlmepriv);\r
  50         BEAMFORMING_CAP         BeamformEntryCap = BEAMFORMING_CAP_NONE;\r
  51         \r
  52         for(i = 0; i < BEAMFORMING_ENTRY_NUM; i++)\r
  53         {\r
  54                 if(     pBeamInfo->beamforming_entry[i].bUsed && \r
  55                         (mac_id == pBeamInfo->beamforming_entry[i].mac_id))\r
  56                 {\r
  57                         BeamformEntryCap =  pBeamInfo->beamforming_entry[i].beamforming_entry_cap;\r
  58                         i = BEAMFORMING_ENTRY_NUM;\r
  59                 }\r
  60         }\r
  61 \r
  62         return BeamformEntryCap;\r
  63 }\r
  64 \r
  65 struct beamforming_entry        *beamforming_get_free_entry(struct mlme_priv *pmlmepriv, u8* idx)\r
  66 {\r
  67         u8      i = 0;\r
  68         struct beamforming_info *pBeamInfo = GET_BEAMFORM_INFO(pmlmepriv);\r
  69 \r
  70         for(i = 0; i < BEAMFORMING_ENTRY_NUM; i++)\r
  71         {\r
  72                 if(pBeamInfo->beamforming_entry[i].bUsed == _FALSE)\r
  73                 {\r
  74                         *idx = i;\r
  75                         return &(pBeamInfo->beamforming_entry[i]);\r
  76                 }       \r
  77         }\r
  78         return NULL;\r
  79 }\r
  80 \r
  81 \r
  82 struct beamforming_entry        *beamforming_add_entry(PADAPTER adapter, u8* ra, u16 aid,\r
  83         u16 mac_id, CHANNEL_WIDTH bw, BEAMFORMING_CAP beamfrom_cap, u8* idx)\r
  84 {\r
  85         struct mlme_priv                        *pmlmepriv = &(adapter->mlmepriv);\r
  86         struct beamforming_entry        *pEntry = beamforming_get_free_entry(pmlmepriv, idx);\r
  87 \r
  88         if(pEntry != NULL)\r
  89         {       \r
  90                 pEntry->bUsed = _TRUE;\r
  91                 pEntry->aid = aid;\r
  92                 pEntry->mac_id = mac_id;\r
  93                 pEntry->sound_bw = bw;\r
  94                 if (check_fwstate(pmlmepriv, WIFI_AP_STATE))\r
  95                 {\r
  96                         u16     BSSID = ((*(adapter_mac_addr(adapter) + 5) & 0xf0) >> 4) ^ \r
  97                                 (*(adapter_mac_addr(adapter) + 5) & 0xf); /* BSSID[44:47] xor BSSID[40:43] */\r
  98                         pEntry->p_aid = (aid + BSSID * 32) & 0x1ff;             // (dec(A) + dec(B)*32) mod 512\r
  99                         pEntry->g_id = 63;\r
 100                 }               \r
 101                 else if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) || check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE))\r
 102                 {\r
 103                         pEntry->p_aid = 0;\r
 104                         pEntry->g_id = 63;\r
 105                 }\r
 106                 else\r
 107                 {\r
 108                         pEntry->p_aid =  ra[5];                                         // BSSID[39:47]\r
 109                         pEntry->p_aid = (pEntry->p_aid << 1) | (ra[4] >> 7 );\r
 110                         pEntry->g_id = 0;\r
 111                 }\r
 112                 _rtw_memcpy(pEntry->mac_addr, ra, ETH_ALEN);\r
 113                 pEntry->bSound = _FALSE;\r
 114 \r
 115                 //3 TODO SW/FW sound period\r
 116                 pEntry->sound_period = 200;\r
 117                 pEntry->beamforming_entry_cap = beamfrom_cap;\r
 118                 pEntry->beamforming_entry_state = BEAMFORMING_ENTRY_STATE_UNINITIALIZE;\r
 119 \r
 120 \r
 121                 pEntry->PreLogSeq = 0;  /*Modified by Jeffery @2015-04-13*/\r
 122                 pEntry->LogSeq = 0;             /*Modified by Jeffery @2014-10-29*/\r
 123                 pEntry->LogRetryCnt = 0;        /*Modified by Jeffery @2014-10-29*/\r
 124                 pEntry->LogSuccess = 0; /*LogSuccess is NOT needed to be accumulated, so  LogSuccessCnt->LogSuccess, 2015-04-13, Jeffery*/\r
 125                 pEntry->ClockResetTimes = 0;    /*Modified by Jeffery @2015-04-13*/\r
 126                 pEntry->LogStatusFailCnt = 0;\r
 127 \r
 128                 return pEntry;\r
 129         }\r
 130         else\r
 131                 return NULL;\r
 132 }\r
 133 \r
 134 BOOLEAN beamforming_remove_entry(struct mlme_priv *pmlmepriv, u8* ra, u8* idx)\r
 135 {\r
 136         struct beamforming_entry        *pEntry = beamforming_get_entry_by_addr(pmlmepriv, ra, idx);\r
 137 \r
 138         if(pEntry != NULL)\r
 139         {       \r
 140                 pEntry->bUsed = _FALSE;\r
 141                 pEntry->beamforming_entry_cap = BEAMFORMING_CAP_NONE;\r
 142                 pEntry->beamforming_entry_state = BEAMFORMING_ENTRY_STATE_UNINITIALIZE;\r
 143                 return _TRUE;\r
 144         }\r
 145         else\r
 146                 return _FALSE;\r
 147 }\r
 148 \r
 149 /* Used for BeamformingStart_V1  */\r
 150 void    beamforming_dym_ndpa_rate(PADAPTER adapter)\r
 151 {\r
 152         u16     NDPARate = MGN_6M;\r
 153         PHAL_DATA_TYPE  pHalData = GET_HAL_DATA(adapter);\r
 154         \r
 155         if(pHalData->MinUndecoratedPWDBForDM > 30) // link RSSI > 30%\r
 156                 NDPARate = MGN_24M;\r
 157         else\r
 158                 NDPARate = MGN_6M;\r
 159 \r
 160         //BW = CHANNEL_WIDTH_20;\r
 161         NDPARate = NDPARate << 8;\r
 162         rtw_hal_set_hwreg(adapter, HW_VAR_SOUNDING_RATE, (u8 *)&NDPARate);\r
 163 }\r
 164 \r
 165 void beamforming_dym_period(PADAPTER Adapter)\r
 166 {\r
 167         u8      Idx;\r
 168         BOOLEAN bChangePeriod = _FALSE;\r
 169         u16     SoundPeriod_SW, SoundPeriod_FW;\r
 170         PHAL_DATA_TYPE  pHalData = GET_HAL_DATA(Adapter);\r
 171         struct dvobj_priv       *pdvobjpriv = adapter_to_dvobj(Adapter);\r
 172         struct beamforming_entry        *pBeamformEntry;\r
 173         struct beamforming_info *pBeamInfo = GET_BEAMFORM_INFO(( &Adapter->mlmepriv));\r
 174         struct sounding_info            *pSoundInfo = &(pBeamInfo->sounding_info);\r
 175         \r
 176         //3 TODO  per-client throughput caculation.\r
 177 \r
 178         if(pdvobjpriv->traffic_stat.cur_tx_tp + pdvobjpriv->traffic_stat.cur_rx_tp > 2)\r
 179         {\r
 180                 SoundPeriod_SW = 32*20;\r
 181                 SoundPeriod_FW = 2;\r
 182         }       \r
 183         else\r
 184         {\r
 185                 SoundPeriod_SW = 32*2000;\r
 186                 SoundPeriod_FW = 200;\r
 187         }       \r
 188 \r
 189         for(Idx = 0; Idx < BEAMFORMING_ENTRY_NUM; Idx++)\r
 190         {\r
 191                 pBeamformEntry = pBeamInfo->beamforming_entry+Idx;\r
 192                 if(pBeamformEntry->bDefaultCSI)\r
 193                 {\r
 194                         SoundPeriod_SW = 32*2000;\r
 195                         SoundPeriod_FW = 200;\r
 196                 }\r
 197 \r
 198                 if(pBeamformEntry->beamforming_entry_cap & (BEAMFORMER_CAP_HT_EXPLICIT |BEAMFORMER_CAP_VHT_SU))\r
 199                 {\r
 200                         if(pSoundInfo->sound_mode == SOUNDING_FW_VHT_TIMER || pSoundInfo->sound_mode == SOUNDING_FW_HT_TIMER)\r
 201                         {                               \r
 202                                 if(pBeamformEntry->sound_period != SoundPeriod_FW)\r
 203                                 {\r
 204                                         pBeamformEntry->sound_period = SoundPeriod_FW;\r
 205                                         bChangePeriod = _TRUE;  // Only FW sounding need to send H2C packet to change sound period. \r
 206                                 }\r
 207                         }\r
 208                         else if(pBeamformEntry->sound_period != SoundPeriod_SW)\r
 209                         {\r
 210                                 pBeamformEntry->sound_period = SoundPeriod_SW;\r
 211                         }\r
 212                 }\r
 213         }\r
 214 \r
 215         if(bChangePeriod)\r
 216                 rtw_hal_set_hwreg(Adapter, HW_VAR_SOUNDING_FW_NDPA, (u8 *)&Idx);\r
 217 }\r
 218 \r
 219 BOOLEAN issue_ht_sw_ndpa_packet(PADAPTER Adapter, u8 *ra, CHANNEL_WIDTH bw, u8 qidx)\r
 220 {\r
 221         struct xmit_frame               *pmgntframe;\r
 222         struct pkt_attrib               *pattrib;\r
 223         struct rtw_ieee80211_hdr        *pwlanhdr;\r
 224         struct xmit_priv                *pxmitpriv = &(Adapter->xmitpriv);\r
 225         struct mlme_ext_priv    *pmlmeext = &Adapter->mlmeextpriv;\r
 226         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);\r
 227         u8      ActionHdr[4] = {ACT_CAT_VENDOR, 0x00, 0xe0, 0x4c};\r
 228         u8      *pframe;\r
 229         u16     *fctrl;\r
 230         u16     duration = 0;\r
 231         u8      aSifsTime = 0;\r
 232         u8      NDPTxRate = 0;\r
 233 \r
 234         DBG_871X("%s: issue_ht_sw_ndpa_packet!\n", __func__);\r
 235 \r
 236         NDPTxRate = MGN_MCS8;\r
 237         DBG_871X("%s: NDPTxRate =%d\n", __func__, NDPTxRate);\r
 238         pmgntframe = alloc_mgtxmitframe(pxmitpriv);\r
 239         \r
 240         if (pmgntframe == NULL)\r
 241                 return _FALSE;\r
 242 \r
 243         /*update attribute*/\r
 244         pattrib = &pmgntframe->attrib;\r
 245         update_mgntframe_attrib(Adapter, pattrib);\r
 246         pattrib->qsel = QSLT_MGNT;\r
 247         pattrib->rate = NDPTxRate;\r
 248         pattrib->bwmode = bw;\r
 249         pattrib->order = 1;\r
 250         pattrib->subtype = WIFI_ACTION_NOACK;\r
 251 \r
 252         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);\r
 253 \r
 254         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;\r
 255 \r
 256         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;\r
 257 \r
 258         fctrl = &pwlanhdr->frame_ctl;\r
 259         *(fctrl) = 0;\r
 260 \r
 261         SetOrderBit(pframe);\r
 262         SetFrameSubType(pframe, WIFI_ACTION_NOACK);\r
 263 \r
 264         _rtw_memcpy(pwlanhdr->addr1, ra, ETH_ALEN);\r
 265         _rtw_memcpy(pwlanhdr->addr2, adapter_mac_addr(Adapter), ETH_ALEN);\r
 266         _rtw_memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);\r
 267 \r
 268         if (pmlmeext->cur_wireless_mode == WIRELESS_11B)\r
 269                 aSifsTime = 10;\r
 270         else\r
 271                 aSifsTime = 16;\r
 272 \r
 273         duration = 2*aSifsTime + 40;\r
 274         \r
 275         if (bw == CHANNEL_WIDTH_40)\r
 276                 duration += 87;\r
 277         else    \r
 278                 duration += 180;\r
 279 \r
 280         SetDuration(pframe, duration);\r
 281 \r
 282         /*HT control field*/\r
 283         SET_HT_CTRL_CSI_STEERING(pframe+24, 3);\r
 284         SET_HT_CTRL_NDP_ANNOUNCEMENT(pframe+24, 1);\r
 285 \r
 286         _rtw_memcpy(pframe+28, ActionHdr, 4);\r
 287 \r
 288         pattrib->pktlen = 32;\r
 289 \r
 290         pattrib->last_txcmdsz = pattrib->pktlen;\r
 291 \r
 292         dump_mgntframe(Adapter, pmgntframe);\r
 293 \r
 294         return _TRUE;\r
 295 \r
 296 \r
 297 }\r
 298 BOOLEAN issue_ht_ndpa_packet(PADAPTER Adapter, u8 *ra, CHANNEL_WIDTH bw, u8 qidx)\r
 299 {\r
 300         struct xmit_frame               *pmgntframe;\r
 301         struct pkt_attrib               *pattrib;\r
 302         struct rtw_ieee80211_hdr        *pwlanhdr;\r
 303         struct xmit_priv                *pxmitpriv = &(Adapter->xmitpriv);\r
 304         struct mlme_ext_priv    *pmlmeext = &Adapter->mlmeextpriv;\r
 305         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);\r
 306         u8      ActionHdr[4] = {ACT_CAT_VENDOR, 0x00, 0xe0, 0x4c};\r
 307         u8      *pframe;\r
 308         u16     *fctrl;\r
 309         u16     duration = 0;\r
 310         u8      aSifsTime = 0;\r
 311 \r
 312         pmgntframe = alloc_mgtxmitframe(pxmitpriv);\r
 313 \r
 314         if (pmgntframe == NULL)\r
 315                 return _FALSE;\r
 316 \r
 317         /*update attribute*/\r
 318         pattrib = &pmgntframe->attrib;\r
 319         update_mgntframe_attrib(Adapter, pattrib);\r
 320 \r
 321         if (qidx == BCN_QUEUE_INX)\r
 322                 pattrib->qsel = QSLT_BEACON;\r
 323         pattrib->rate = MGN_MCS8;\r
 324         pattrib->bwmode = bw;\r
 325         pattrib->order = 1;\r
 326         pattrib->subtype = WIFI_ACTION_NOACK;\r
 327 \r
 328         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);\r
 329 \r
 330         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;\r
 331 \r
 332         pwlanhdr = (struct rtw_ieee80211_hdr*)pframe;\r
 333 \r
 334         fctrl = &pwlanhdr->frame_ctl;\r
 335         *(fctrl) = 0;\r
 336 \r
 337         SetOrderBit(pframe);\r
 338         SetFrameSubType(pframe, WIFI_ACTION_NOACK);\r
 339 \r
 340         _rtw_memcpy(pwlanhdr->addr1, ra, ETH_ALEN);\r
 341         _rtw_memcpy(pwlanhdr->addr2, adapter_mac_addr(Adapter), ETH_ALEN);\r
 342         _rtw_memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);\r
 343 \r
 344         if( pmlmeext->cur_wireless_mode == WIRELESS_11B)\r
 345                 aSifsTime = 10;\r
 346         else\r
 347                 aSifsTime = 16;\r
 348 \r
 349         duration = 2*aSifsTime + 40;\r
 350         \r
 351         if(bw == CHANNEL_WIDTH_40)\r
 352                 duration+= 87;\r
 353         else    \r
 354                 duration+= 180;\r
 355 \r
 356         SetDuration(pframe, duration);\r
 357 \r
 358         //HT control field\r
 359         SET_HT_CTRL_CSI_STEERING(pframe+24, 3);\r
 360         SET_HT_CTRL_NDP_ANNOUNCEMENT(pframe+24, 1);\r
 361 \r
 362         _rtw_memcpy(pframe+28, ActionHdr, 4);\r
 363 \r
 364         pattrib->pktlen = 32;\r
 365 \r
 366         pattrib->last_txcmdsz = pattrib->pktlen;\r
 367 \r
 368         dump_mgntframe(Adapter, pmgntframe);\r
 369 \r
 370         return _TRUE;\r
 371 }\r
 372 \r
 373 BOOLEAN beamforming_send_ht_ndpa_packet(PADAPTER Adapter, u8 *ra, CHANNEL_WIDTH bw, u8 qidx)\r
 374 {\r
 375         return issue_ht_ndpa_packet(Adapter, ra, bw, qidx);\r
 376 }\r
 377 BOOLEAN issue_vht_sw_ndpa_packet(PADAPTER Adapter, u8 *ra, u16 aid, CHANNEL_WIDTH bw, u8 qidx)\r
 378 {\r
 379         struct xmit_frame               *pmgntframe;\r
 380         struct pkt_attrib               *pattrib;\r
 381         struct rtw_ieee80211_hdr        *pwlanhdr;\r
 382         struct xmit_priv                *pxmitpriv = &(Adapter->xmitpriv);\r
 383         struct mlme_ext_priv    *pmlmeext = &Adapter->mlmeextpriv;\r
 384         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);\r
 385         struct mlme_priv                *pmlmepriv = &(Adapter->mlmepriv);\r
 386         struct beamforming_info *pBeamInfo = GET_BEAMFORM_INFO(pmlmepriv);\r
 387         struct rtw_ndpa_sta_info        sta_info;\r
 388         u8               NDPTxRate = 0;\r
 389         \r
 390         u8      *pframe;\r
 391         u16     *fctrl;\r
 392         u16     duration = 0;\r
 393         u8      sequence = 0, aSifsTime = 0;\r
 394 \r
 395         DBG_871X("%s: issue_vht_sw_ndpa_packet!\n", __func__);\r
 396 \r
 397         \r
 398         NDPTxRate = MGN_VHT2SS_MCS0;\r
 399         DBG_871X("%s: NDPTxRate =%d\n", __func__, NDPTxRate);\r
 400         pmgntframe = alloc_mgtxmitframe(pxmitpriv);\r
 401 \r
 402         if (pmgntframe == NULL) {\r
 403                 DBG_871X("%s, alloc mgnt frame fail\n", __func__);\r
 404                 return _FALSE;\r
 405         }\r
 406 \r
 407         /*update attribute*/\r
 408         pattrib = &pmgntframe->attrib;\r
 409         update_mgntframe_attrib(Adapter, pattrib);\r
 410         pattrib->qsel = QSLT_MGNT;\r
 411         pattrib->rate = NDPTxRate;\r
 412         pattrib->bwmode = bw;\r
 413         pattrib->subtype = WIFI_NDPA;\r
 414 \r
 415         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);\r
 416 \r
 417         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;\r
 418 \r
 419         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;\r
 420 \r
 421         fctrl = &pwlanhdr->frame_ctl;\r
 422         *(fctrl) = 0;\r
 423 \r
 424         SetFrameSubType(pframe, WIFI_NDPA);\r
 425 \r
 426         _rtw_memcpy(pwlanhdr->addr1, ra, ETH_ALEN);\r
 427         _rtw_memcpy(pwlanhdr->addr2, adapter_mac_addr(Adapter), ETH_ALEN);\r
 428 \r
 429         if (IsSupported5G(pmlmeext->cur_wireless_mode) || IsSupportedHT(pmlmeext->cur_wireless_mode))\r
 430                 aSifsTime = 16;\r
 431         else\r
 432                 aSifsTime = 10;\r
 433 \r
 434         duration = 2*aSifsTime + 44;\r
 435         \r
 436         if (bw == CHANNEL_WIDTH_80)\r
 437                 duration += 40;\r
 438         else if (bw == CHANNEL_WIDTH_40)\r
 439                 duration += 87;\r
 440         else    \r
 441                 duration += 180;\r
 442 \r
 443         SetDuration(pframe, duration);\r
 444         \r
 445         sequence = pBeamInfo->sounding_sequence << 2;\r
 446         if (pBeamInfo->sounding_sequence >= 0x3f)\r
 447                 pBeamInfo->sounding_sequence = 0;\r
 448         else\r
 449                 pBeamInfo->sounding_sequence++;\r
 450 \r
 451         _rtw_memcpy(pframe+16, &sequence, 1);\r
 452         if (((pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE) || ((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE))\r
 453                 aid = 0;                \r
 454 \r
 455         sta_info.aid = aid;\r
 456         sta_info.feedback_type = 0;\r
 457         sta_info.nc_index = 0;\r
 458         \r
 459         _rtw_memcpy(pframe+17, (u8 *)&sta_info, 2);\r
 460 \r
 461         pattrib->pktlen = 19;\r
 462 \r
 463         pattrib->last_txcmdsz = pattrib->pktlen;\r
 464 \r
 465         dump_mgntframe(Adapter, pmgntframe);\r
 466         \r
 467 \r
 468         return _TRUE;\r
 469 \r
 470 }\r
 471 BOOLEAN issue_vht_ndpa_packet(PADAPTER Adapter, u8 *ra, u16 aid, CHANNEL_WIDTH bw, u8 qidx)\r
 472 {\r
 473         struct xmit_frame               *pmgntframe;\r
 474         struct pkt_attrib               *pattrib;\r
 475         struct rtw_ieee80211_hdr        *pwlanhdr;\r
 476         struct xmit_priv                *pxmitpriv = &(Adapter->xmitpriv);\r
 477         struct mlme_ext_priv    *pmlmeext = &Adapter->mlmeextpriv;\r
 478         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);\r
 479         struct mlme_priv                *pmlmepriv = &(Adapter->mlmepriv);\r
 480         struct beamforming_info *pBeamInfo = GET_BEAMFORM_INFO(pmlmepriv);\r
 481         struct rtw_ndpa_sta_info        sta_info;\r
 482         u8      *pframe;\r
 483         u16     *fctrl;\r
 484         u16     duration = 0;\r
 485         u8      sequence = 0, aSifsTime = 0;\r
 486 \r
 487         if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)\r
 488                 return _FALSE;\r
 489 \r
 490         /*update attribute*/\r
 491         pattrib = &pmgntframe->attrib;\r
 492         update_mgntframe_attrib(Adapter, pattrib);\r
 493 \r
 494         if (qidx == BCN_QUEUE_INX)\r
 495                 pattrib->qsel = QSLT_BEACON;\r
 496         pattrib->rate = MGN_VHT2SS_MCS0;\r
 497         pattrib->bwmode = bw;\r
 498         pattrib->subtype = WIFI_NDPA;\r
 499 \r
 500         _rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);\r
 501 \r
 502         pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;\r
 503 \r
 504         pwlanhdr = (struct rtw_ieee80211_hdr*)pframe;\r
 505 \r
 506         fctrl = &pwlanhdr->frame_ctl;\r
 507         *(fctrl) = 0;\r
 508 \r
 509         SetFrameSubType(pframe, WIFI_NDPA);\r
 510 \r
 511         _rtw_memcpy(pwlanhdr->addr1, ra, ETH_ALEN);\r
 512         _rtw_memcpy(pwlanhdr->addr2, adapter_mac_addr(Adapter), ETH_ALEN);\r
 513 \r
 514         if (IsSupported5G(pmlmeext->cur_wireless_mode) || IsSupportedHT(pmlmeext->cur_wireless_mode))\r
 515                 aSifsTime = 16;\r
 516         else\r
 517                 aSifsTime = 10;\r
 518 \r
 519         duration = 2*aSifsTime + 44;\r
 520         \r
 521         if(bw == CHANNEL_WIDTH_80)\r
 522                 duration += 40;\r
 523         else if(bw == CHANNEL_WIDTH_40)\r
 524                 duration+= 87;\r
 525         else    \r
 526                 duration+= 180;\r
 527 \r
 528         SetDuration(pframe, duration);\r
 529 \r
 530         sequence = pBeamInfo->sounding_sequence<< 2;\r
 531         if (pBeamInfo->sounding_sequence >= 0x3f)\r
 532                 pBeamInfo->sounding_sequence = 0;\r
 533         else\r
 534                 pBeamInfo->sounding_sequence++;\r
 535 \r
 536         _rtw_memcpy(pframe+16, &sequence,1);\r
 537 \r
 538         if(((pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE) || ((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE))\r
 539                 aid = 0;                \r
 540 \r
 541         sta_info.aid = aid;\r
 542         sta_info.feedback_type = 0;\r
 543         sta_info.nc_index= 0;\r
 544         \r
 545         _rtw_memcpy(pframe+17, (u8 *)&sta_info, 2);\r
 546 \r
 547         pattrib->pktlen = 19;\r
 548 \r
 549         pattrib->last_txcmdsz = pattrib->pktlen;\r
 550 \r
 551         dump_mgntframe(Adapter, pmgntframe);\r
 552 \r
 553         return _TRUE;\r
 554 }\r
 555 \r
 556 BOOLEAN beamforming_send_vht_ndpa_packet(PADAPTER Adapter, u8 *ra, u16 aid, CHANNEL_WIDTH bw, u8 qidx)\r
 557 {\r
 558         return issue_vht_ndpa_packet(Adapter, ra, aid, bw, qidx);\r
 559 }\r
 560 \r
 561 BOOLEAN beamfomring_bSounding(struct beamforming_info *pBeamInfo)\r
 562 {\r
 563         BOOLEAN         bSounding = _FALSE;\r
 564 \r
 565         if(( beamforming_get_beamform_cap(pBeamInfo) & BEAMFORMER_CAP) == 0)\r
 566                 bSounding = _FALSE;\r
 567         else \r
 568                 bSounding = _TRUE;\r
 569 \r
 570         return bSounding;\r
 571 }\r
 572 \r
 573 u8      beamforming_sounding_idx(struct beamforming_info *pBeamInfo)\r
 574 {\r
 575         u8      idx = 0;\r
 576         u8      i;\r
 577 \r
 578         for(i = 0; i < BEAMFORMING_ENTRY_NUM; i++)\r
 579         {\r
 580                 if (pBeamInfo->beamforming_entry[i].bUsed &&\r
 581                         (_FALSE == pBeamInfo->beamforming_entry[i].bSound))\r
 582                 {\r
 583                         idx = i;\r
 584                         break;\r
 585                 }\r
 586         }\r
 587 \r
 588         return idx;\r
 589 }\r
 590 \r
 591 SOUNDING_MODE   beamforming_sounding_mode(struct beamforming_info *pBeamInfo, u8 idx)\r
 592 {\r
 593         struct beamforming_entry        BeamEntry = pBeamInfo->beamforming_entry[idx];\r
 594         SOUNDING_MODE   mode;\r
 595 \r
 596         if(BeamEntry.beamforming_entry_cap & BEAMFORMER_CAP_VHT_SU)\r
 597         {\r
 598                 mode = SOUNDING_FW_VHT_TIMER;\r
 599         }\r
 600         else if(BeamEntry.beamforming_entry_cap & BEAMFORMER_CAP_HT_EXPLICIT)\r
 601         {\r
 602                 mode = SOUNDING_FW_HT_TIMER;\r
 603         }\r
 604         else\r
 605         {\r
 606                 mode = SOUNDING_STOP_All_TIMER;\r
 607         }\r
 608 \r
 609         return mode;\r
 610 }\r
 611 \r
 612 u16     beamforming_sounding_time(struct beamforming_info *pBeamInfo, SOUNDING_MODE mode, u8 idx)\r
 613 {\r
 614         u16                                             sounding_time = 0xffff;\r
 615         struct beamforming_entry        BeamEntry = pBeamInfo->beamforming_entry[idx];\r
 616 \r
 617         sounding_time = BeamEntry.sound_period;\r
 618 \r
 619         return sounding_time;\r
 620 }\r
 621 \r
 622 CHANNEL_WIDTH   beamforming_sounding_bw(struct beamforming_info *pBeamInfo, SOUNDING_MODE mode, u8 idx)\r
 623 {\r
 624         CHANNEL_WIDTH                           sounding_bw = CHANNEL_WIDTH_20;\r
 625         struct beamforming_entry                BeamEntry = pBeamInfo->beamforming_entry[idx];\r
 626 \r
 627         sounding_bw = BeamEntry.sound_bw;\r
 628 \r
 629         return sounding_bw;\r
 630 }\r
 631 \r
 632 BOOLEAN beamforming_select_beam_entry(struct beamforming_info *pBeamInfo)\r
 633 {\r
 634         struct sounding_info            *pSoundInfo = &(pBeamInfo->sounding_info);\r
 635 \r
 636         pSoundInfo->sound_idx = beamforming_sounding_idx(pBeamInfo);\r
 637 \r
 638         if(pSoundInfo->sound_idx < BEAMFORMING_ENTRY_NUM)\r
 639                 pSoundInfo->sound_mode = beamforming_sounding_mode(pBeamInfo, pSoundInfo->sound_idx);\r
 640         else\r
 641                 pSoundInfo->sound_mode = SOUNDING_STOP_All_TIMER;\r
 642         \r
 643         if(SOUNDING_STOP_All_TIMER == pSoundInfo->sound_mode)\r
 644         {\r
 645                 return _FALSE;\r
 646         }\r
 647         else\r
 648         {\r
 649                 pSoundInfo->sound_bw = beamforming_sounding_bw(pBeamInfo, pSoundInfo->sound_mode, pSoundInfo->sound_idx );\r
 650                 pSoundInfo->sound_period = beamforming_sounding_time(pBeamInfo, pSoundInfo->sound_mode, pSoundInfo->sound_idx );\r
 651                 return _TRUE;\r
 652         }\r
 653 }\r
 654 \r
 655 BOOLEAN beamforming_start_fw(PADAPTER adapter, u8 idx)\r
 656 {\r
 657         u8                                              *RA = NULL;\r
 658         struct beamforming_entry        *pEntry;\r
 659         BOOLEAN                                 ret = _TRUE;\r
 660         struct mlme_priv                        *pmlmepriv = &(adapter->mlmepriv);\r
 661         struct beamforming_info *pBeamInfo = GET_BEAMFORM_INFO(pmlmepriv);\r
 662 \r
 663         pEntry = &(pBeamInfo->beamforming_entry[idx]);\r
 664         if(pEntry->bUsed == _FALSE)\r
 665         {\r
 666                 DBG_871X("Skip Beamforming, no entry for Idx =%d\n", idx);\r
 667                 return _FALSE;\r
 668         }\r
 669 \r
 670         pEntry->beamforming_entry_state = BEAMFORMING_ENTRY_STATE_PROGRESSING;\r
 671         pEntry->bSound = _TRUE;\r
 672         rtw_hal_set_hwreg(adapter, HW_VAR_SOUNDING_FW_NDPA, (u8 *)&idx);\r
 673 \r
 674         return _TRUE;\r
 675 }\r
 676 \r
 677 void    beamforming_end_fw(PADAPTER adapter)\r
 678 {\r
 679         u8      idx = 0;\r
 680 \r
 681         rtw_hal_set_hwreg(adapter, HW_VAR_SOUNDING_FW_NDPA, (u8 *)&idx);\r
 682 \r
 683         DBG_871X("%s\n", __FUNCTION__);\r
 684 }\r
 685 \r
 686 BOOLEAN beamforming_start_period(PADAPTER adapter)\r
 687 {\r
 688         BOOLEAN ret = _TRUE;\r
 689         struct mlme_priv                        *pmlmepriv = &(adapter->mlmepriv);\r
 690         struct beamforming_info *pBeamInfo = GET_BEAMFORM_INFO(pmlmepriv);\r
 691         struct sounding_info            *pSoundInfo = &(pBeamInfo->sounding_info);\r
 692 \r
 693         beamforming_dym_ndpa_rate(adapter);\r
 694 \r
 695         beamforming_select_beam_entry(pBeamInfo);\r
 696 \r
 697         if(pSoundInfo->sound_mode == SOUNDING_FW_VHT_TIMER || pSoundInfo->sound_mode == SOUNDING_FW_HT_TIMER)\r
 698         {\r
 699                 ret = beamforming_start_fw(adapter, pSoundInfo->sound_idx);\r
 700         }\r
 701         else\r
 702         {\r
 703                 ret = _FALSE;\r
 704         }\r
 705 \r
 706         DBG_871X("%s Idx %d Mode %d BW %d Period %d\n", __FUNCTION__, \r
 707                         pSoundInfo->sound_idx, pSoundInfo->sound_mode, pSoundInfo->sound_bw, pSoundInfo->sound_period);\r
 708 \r
 709         return ret;\r
 710 }\r
 711 \r
 712 void    beamforming_end_period(PADAPTER adapter)\r
 713 {\r
 714         u8                                              idx = 0;\r
 715         struct beamforming_entry        *pBeamformEntry;\r
 716         struct mlme_priv                        *pmlmepriv = &(adapter->mlmepriv);\r
 717         struct beamforming_info *pBeamInfo = GET_BEAMFORM_INFO(pmlmepriv);\r
 718         struct sounding_info            *pSoundInfo = &(pBeamInfo->sounding_info);\r
 719 \r
 720 \r
 721         if(pSoundInfo->sound_mode == SOUNDING_FW_VHT_TIMER || pSoundInfo->sound_mode == SOUNDING_FW_HT_TIMER)\r
 722         {               \r
 723                 beamforming_end_fw(adapter);\r
 724         }\r
 725 }\r
 726 \r
 727 void    beamforming_notify(PADAPTER adapter)\r
 728 {\r
 729         BOOLEAN         bSounding = _FALSE;\r
 730         struct beamforming_info *pBeamInfo = GET_BEAMFORM_INFO(&(adapter->mlmepriv));\r
 731 \r
 732         bSounding = beamfomring_bSounding(pBeamInfo);\r
 733         \r
 734         if(pBeamInfo->beamforming_state == BEAMFORMING_STATE_IDLE)\r
 735         {\r
 736                 if(bSounding)\r
 737                 {                       \r
 738                         if(beamforming_start_period(adapter) == _TRUE)\r
 739                                 pBeamInfo->beamforming_state = BEAMFORMING_STATE_START;\r
 740                 }\r
 741         }\r
 742         else if(pBeamInfo->beamforming_state == BEAMFORMING_STATE_START)\r
 743         {\r
 744                 if(bSounding)\r
 745                 {\r
 746                         if(beamforming_start_period(adapter) == _FALSE)\r
 747                                 pBeamInfo->beamforming_state = BEAMFORMING_STATE_END;\r
 748                 }\r
 749                 else\r
 750                 {\r
 751                         beamforming_end_period(adapter);\r
 752                         pBeamInfo->beamforming_state = BEAMFORMING_STATE_END;\r
 753                 }\r
 754         }\r
 755         else if(pBeamInfo->beamforming_state == BEAMFORMING_STATE_END)\r
 756         {\r
 757                 if(bSounding)\r
 758                 {\r
 759                         if(beamforming_start_period(adapter) == _TRUE)\r
 760                                 pBeamInfo->beamforming_state = BEAMFORMING_STATE_START;\r
 761                 }\r
 762         }\r
 763         else\r
 764         {\r
 765                 DBG_871X("%s BeamformState %d\n", __FUNCTION__, pBeamInfo->beamforming_state);\r
 766         }\r
 767 \r
 768         DBG_871X("%s BeamformState %d bSounding %d\n", __FUNCTION__, pBeamInfo->beamforming_state, bSounding);\r
 769 }\r
 770 \r
 771 BOOLEAN beamforming_init_entry(PADAPTER adapter, struct sta_info *psta, u8* idx)\r
 772 {\r
 773         struct mlme_priv        *pmlmepriv = &(adapter->mlmepriv);\r
 774         struct ht_priv          *phtpriv = &(pmlmepriv->htpriv);\r
 775 #ifdef CONFIG_80211AC_VHT       \r
 776         struct vht_priv         *pvhtpriv = &(pmlmepriv->vhtpriv);\r
 777 #endif\r
 778         struct mlme_ext_priv    *pmlmeext = &(adapter->mlmeextpriv);\r
 779         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);\r
 780         struct beamforming_entry        *pBeamformEntry = NULL;\r
 781         u8      *ra; \r
 782         u16     aid, mac_id;\r
 783         u8      wireless_mode;\r
 784         CHANNEL_WIDTH   bw = CHANNEL_WIDTH_20;\r
 785         BEAMFORMING_CAP beamform_cap = BEAMFORMING_CAP_NONE;\r
 786 \r
 787         // The current setting does not support Beaforming\r
 788         if (0 == phtpriv->beamform_cap \r
 789 #ifdef CONFIG_80211AC_VHT\r
 790                 && 0 == pvhtpriv->beamform_cap\r
 791 #endif\r
 792                 ) {\r
 793                 DBG_871X("The configuration disabled Beamforming! Skip...\n");\r
 794                 return _FALSE;\r
 795         }\r
 796 \r
 797         aid = psta->aid;\r
 798         ra = psta->hwaddr;\r
 799         mac_id = psta->mac_id;\r
 800         wireless_mode = psta->wireless_mode;\r
 801         bw = psta->bw_mode;\r
 802 \r
 803         if (IsSupportedHT(wireless_mode) || IsSupportedVHT(wireless_mode)) {\r
 804                 //3 // HT\r
 805                 u8      cur_beamform;\r
 806 \r
 807                 cur_beamform = psta->htpriv.beamform_cap;\r
 808 \r
 809                 // We are Beamformee because the STA is Beamformer\r
 810                 if(TEST_FLAG(cur_beamform, BEAMFORMING_HT_BEAMFORMER_ENABLE))\r
 811                         beamform_cap =(BEAMFORMING_CAP)(beamform_cap |BEAMFORMEE_CAP_HT_EXPLICIT);\r
 812 \r
 813                 // We are Beamformer because the STA is Beamformee\r
 814                 if(TEST_FLAG(cur_beamform, BEAMFORMING_HT_BEAMFORMEE_ENABLE))\r
 815                         beamform_cap =(BEAMFORMING_CAP)(beamform_cap | BEAMFORMER_CAP_HT_EXPLICIT);\r
 816 #ifdef CONFIG_80211AC_VHT\r
 817                 if (IsSupportedVHT(wireless_mode)) {\r
 818                         //3 // VHT\r
 819                         cur_beamform = psta->vhtpriv.beamform_cap;\r
 820 \r
 821                         // We are Beamformee because the STA is Beamformer\r
 822                         if(TEST_FLAG(cur_beamform, BEAMFORMING_VHT_BEAMFORMER_ENABLE))\r
 823                                 beamform_cap =(BEAMFORMING_CAP)(beamform_cap |BEAMFORMEE_CAP_VHT_SU);\r
 824                         // We are Beamformer because the STA is Beamformee\r
 825                         if(TEST_FLAG(cur_beamform, BEAMFORMING_VHT_BEAMFORMEE_ENABLE))\r
 826                                 beamform_cap =(BEAMFORMING_CAP)(beamform_cap |BEAMFORMER_CAP_VHT_SU);\r
 827                 }\r
 828 #endif //CONFIG_80211AC_VHT\r
 829 \r
 830                 if(beamform_cap == BEAMFORMING_CAP_NONE)\r
 831                         return _FALSE;\r
 832                 \r
 833                 DBG_871X("Beamforming Config Capability = 0x%02X\n", beamform_cap);\r
 834 \r
 835                 pBeamformEntry = beamforming_get_entry_by_addr(pmlmepriv, ra, idx);\r
 836                 if (pBeamformEntry == NULL) {\r
 837                         pBeamformEntry = beamforming_add_entry(adapter, ra, aid, mac_id, bw, beamform_cap, idx);\r
 838                         if(pBeamformEntry == NULL)\r
 839                                 return _FALSE;\r
 840                         else\r
 841                                 pBeamformEntry->beamforming_entry_state = BEAMFORMING_ENTRY_STATE_INITIALIZEING;\r
 842                 } else {\r
 843                         // Entry has been created. If entry is initialing or progressing then errors occur.\r
 844                         if (pBeamformEntry->beamforming_entry_state != BEAMFORMING_ENTRY_STATE_INITIALIZED && \r
 845                                 pBeamformEntry->beamforming_entry_state != BEAMFORMING_ENTRY_STATE_PROGRESSED) {\r
 846                                 DBG_871X("Error State of Beamforming");\r
 847                                 return _FALSE;\r
 848                         } else {\r
 849                                 pBeamformEntry->beamforming_entry_state = BEAMFORMING_ENTRY_STATE_INITIALIZEING;\r
 850                         }\r
 851                 }\r
 852 \r
 853                 pBeamformEntry->beamforming_entry_state = BEAMFORMING_ENTRY_STATE_INITIALIZED;\r
 854                 psta->txbf_paid = pBeamformEntry->p_aid;\r
 855                 psta->txbf_gid = pBeamformEntry->g_id;\r
 856 \r
 857                 DBG_871X("%s Idx %d\n", __FUNCTION__, *idx);\r
 858         } else {\r
 859                 return _FALSE;\r
 860         }\r
 861 \r
 862         return _SUCCESS;\r
 863 }\r
 864 \r
 865 void    beamforming_deinit_entry(PADAPTER adapter, u8* ra)\r
 866 {\r
 867         u8      idx = 0;\r
 868         struct mlme_priv *pmlmepriv = &(adapter->mlmepriv);\r
 869         \r
 870         if(beamforming_remove_entry(pmlmepriv, ra, &idx) == _TRUE)\r
 871         {\r
 872                 rtw_hal_set_hwreg(adapter, HW_VAR_SOUNDING_LEAVE, (u8 *)&idx);\r
 873         }\r
 874 \r
 875         DBG_871X("%s Idx %d\n", __FUNCTION__, idx);\r
 876 }\r
 877 \r
 878 void    beamforming_reset(PADAPTER adapter)\r
 879 {\r
 880         u8      idx = 0;\r
 881         struct mlme_priv                        *pmlmepriv = &(adapter->mlmepriv);\r
 882         struct beamforming_info *pBeamInfo = GET_BEAMFORM_INFO(pmlmepriv);\r
 883 \r
 884         for(idx = 0; idx < BEAMFORMING_ENTRY_NUM; idx++)\r
 885         {\r
 886                 if(pBeamInfo->beamforming_entry[idx].bUsed == _TRUE)\r
 887                 {\r
 888                         pBeamInfo->beamforming_entry[idx].bUsed = _FALSE;\r
 889                         pBeamInfo->beamforming_entry[idx].beamforming_entry_cap = BEAMFORMING_CAP_NONE;\r
 890                         pBeamInfo->beamforming_entry[idx].beamforming_entry_state= BEAMFORMING_ENTRY_STATE_UNINITIALIZE;\r
 891                         rtw_hal_set_hwreg(adapter, HW_VAR_SOUNDING_LEAVE, (u8 *)&idx);\r
 892                 }\r
 893         }\r
 894 \r
 895         DBG_871X("%s\n", __FUNCTION__);\r
 896 }\r
 897 \r
 898 void beamforming_sounding_fail(PADAPTER Adapter)\r
 899 {\r
 900         struct mlme_priv                        *pmlmepriv = &(Adapter->mlmepriv);\r
 901         struct beamforming_info *pBeamInfo = GET_BEAMFORM_INFO(pmlmepriv);\r
 902         struct beamforming_entry        *pEntry = &(pBeamInfo->beamforming_entry[pBeamInfo->beamforming_cur_idx]);\r
 903 \r
 904         pEntry->bSound = _FALSE;\r
 905         rtw_hal_set_hwreg(Adapter, HW_VAR_SOUNDING_FW_NDPA, (u8 *)&pBeamInfo->beamforming_cur_idx);\r
 906         beamforming_deinit_entry(Adapter, pEntry->mac_addr);\r
 907 }\r
 908 \r
 909 void    beamforming_check_sounding_success(PADAPTER Adapter,BOOLEAN status)\r
 910 {\r
 911         struct mlme_priv                        *pmlmepriv = &(Adapter->mlmepriv);\r
 912         struct beamforming_info *pBeamInfo = GET_BEAMFORM_INFO(pmlmepriv);\r
 913         struct beamforming_entry        *pEntry = &(pBeamInfo->beamforming_entry[pBeamInfo->beamforming_cur_idx]);\r
 914 \r
 915         if(status == 1)\r
 916         {\r
 917                 pEntry->LogStatusFailCnt = 0;\r
 918         }       \r
 919         else\r
 920         {\r
 921                 pEntry->LogStatusFailCnt++;\r
 922                 DBG_871X("%s LogStatusFailCnt %d\n", __FUNCTION__, pEntry->LogStatusFailCnt);\r
 923         }\r
 924         if(pEntry->LogStatusFailCnt > 20)\r
 925         {\r
 926                 DBG_871X("%s LogStatusFailCnt > 20, Stop SOUNDING\n", __FUNCTION__);\r
 927                 //pEntry->bSound = _FALSE;\r
 928                 //rtw_hal_set_hwreg(Adapter, HW_VAR_SOUNDING_FW_NDPA, (u8 *)&pBeamInfo->beamforming_cur_idx);\r
 929                 //beamforming_deinit_entry(Adapter, pEntry->mac_addr);\r
 930                 beamforming_wk_cmd(Adapter, BEAMFORMING_CTRL_SOUNDING_FAIL, NULL, 0, 1);\r
 931         }\r
 932 }\r
 933 \r
 934 void    beamforming_enter(PADAPTER adapter, PVOID psta)\r
 935 {\r
 936         u8      idx = 0xff;\r
 937 \r
 938         if(beamforming_init_entry(adapter, (struct sta_info *)psta, &idx))\r
 939                 rtw_hal_set_hwreg(adapter, HW_VAR_SOUNDING_ENTER, (u8 *)&idx);\r
 940 \r
 941         //DBG_871X("%s Idx %d\n", __FUNCTION__, idx);\r
 942 }\r
 943 \r
 944 void    beamforming_leave(PADAPTER adapter,u8* ra)\r
 945 {\r
 946         if(ra == NULL)\r
 947                 beamforming_reset(adapter);\r
 948         else\r
 949                 beamforming_deinit_entry(adapter, ra);\r
 950 \r
 951         beamforming_notify(adapter);\r
 952 }\r
 953 \r
 954 BEAMFORMING_CAP beamforming_get_beamform_cap(struct beamforming_info    *pBeamInfo)\r
 955 {\r
 956         u8      i;\r
 957         BOOLEAN                                 bSelfBeamformer = _FALSE;\r
 958         BOOLEAN                                 bSelfBeamformee = _FALSE;\r
 959         struct beamforming_entry        beamforming_entry;\r
 960         BEAMFORMING_CAP                 beamform_cap = BEAMFORMING_CAP_NONE;\r
 961 \r
 962         for(i = 0; i < BEAMFORMING_ENTRY_NUM; i++)\r
 963         {\r
 964                 beamforming_entry = pBeamInfo->beamforming_entry[i];\r
 965 \r
 966                 if(beamforming_entry.bUsed)\r
 967                 {\r
 968                         if( (beamforming_entry.beamforming_entry_cap & BEAMFORMEE_CAP_VHT_SU) ||\r
 969                                 (beamforming_entry.beamforming_entry_cap & BEAMFORMEE_CAP_HT_EXPLICIT))\r
 970                                 bSelfBeamformee = _TRUE;\r
 971                         if( (beamforming_entry.beamforming_entry_cap & BEAMFORMER_CAP_VHT_SU) ||\r
 972                                 (beamforming_entry.beamforming_entry_cap & BEAMFORMER_CAP_HT_EXPLICIT))\r
 973                                 bSelfBeamformer = _TRUE;\r
 974                 }\r
 975 \r
 976                 if(bSelfBeamformer && bSelfBeamformee)\r
 977                         i = BEAMFORMING_ENTRY_NUM;\r
 978         }\r
 979 \r
 980         if(bSelfBeamformer)\r
 981                 beamform_cap |= BEAMFORMER_CAP;\r
 982         if(bSelfBeamformee)\r
 983                 beamform_cap |= BEAMFORMEE_CAP;\r
 984 \r
 985         return beamform_cap;\r
 986 }\r
 987 \r
 988 void    beamforming_watchdog(PADAPTER Adapter)\r
 989 {\r
 990         struct beamforming_info *pBeamInfo = GET_BEAMFORM_INFO(( &(Adapter->mlmepriv)));\r
 991 \r
 992         if(pBeamInfo->beamforming_state != BEAMFORMING_STATE_START)\r
 993                 return;\r
 994 \r
 995         beamforming_dym_period(Adapter);\r
 996         beamforming_dym_ndpa_rate(Adapter);\r
 997 }\r
 998 #endif/* #if (BEAMFORMING_SUPPORT ==0) - for diver defined beamforming*/\r
 999 \r
1000 u32     beamforming_get_report_frame(PADAPTER    Adapter, union recv_frame *precv_frame)\r
1001 {\r
1002         u32     ret = _SUCCESS;\r
1003 #if (BEAMFORMING_SUPPORT == 1)\r
1004         PHAL_DATA_TYPE  pHalData = GET_HAL_DATA(Adapter);\r
1005         PDM_ODM_T               pDM_Odm = &(pHalData->odmpriv);\r
1006 \r
1007         ret = Beamforming_GetReportFrame(pDM_Odm, precv_frame);\r
1008         \r
1009 #else /*(BEAMFORMING_SUPPORT == 0)- for drv beamfoming*/\r
1010         struct beamforming_entry        *pBeamformEntry = NULL;\r
1011         struct mlme_priv                        *pmlmepriv = &(Adapter->mlmepriv);\r
1012         u8      *pframe = precv_frame->u.hdr.rx_data;\r
1013         u32     frame_len = precv_frame->u.hdr.len;\r
1014         u8      *ta;\r
1015         u8      idx, offset;\r
1016         \r
1017         /*DBG_871X("beamforming_get_report_frame\n");*/\r
1018 \r
1019         /*Memory comparison to see if CSI report is the same with previous one*/\r
1020         ta = GetAddr2Ptr(pframe);\r
1021         pBeamformEntry = beamforming_get_entry_by_addr(pmlmepriv, ta, &idx);\r
1022         if (pBeamformEntry->beamforming_entry_cap & BEAMFORMER_CAP_VHT_SU)\r
1023                 offset = 31;    /*24+(1+1+3)+2  MAC header+(Category+ActionCode+MIMOControlField)+SNR(Nc=2)*/\r
1024         else if (pBeamformEntry->beamforming_entry_cap & BEAMFORMER_CAP_HT_EXPLICIT)\r
1025                 offset = 34;    /*24+(1+1+6)+2  MAC header+(Category+ActionCode+MIMOControlField)+SNR(Nc=2)*/\r
1026         else\r
1027                 return ret;\r
1028 \r
1029         /*DBG_871X("%s MacId %d offset=%d\n", __FUNCTION__, pBeamformEntry->mac_id, offset);*/\r
1030 \r
1031         if (_rtw_memcmp(pBeamformEntry->PreCsiReport + offset, pframe+offset, frame_len-offset) == _FALSE)\r
1032                 pBeamformEntry->DefaultCsiCnt = 0;\r
1033         else \r
1034                 pBeamformEntry->DefaultCsiCnt++;\r
1035         \r
1036         _rtw_memcpy(&pBeamformEntry->PreCsiReport, pframe, frame_len);\r
1037 \r
1038         pBeamformEntry->bDefaultCSI = _FALSE;\r
1039 \r
1040         if (pBeamformEntry->DefaultCsiCnt > 20)\r
1041                 pBeamformEntry->bDefaultCSI = _TRUE;\r
1042         else\r
1043                 pBeamformEntry->bDefaultCSI = _FALSE;\r
1044 #endif\r
1045         return ret;\r
1046 }\r
1047 \r
1048 void    beamforming_get_ndpa_frame(PADAPTER      Adapter, union recv_frame *precv_frame)\r
1049 {\r
1050 #if (BEAMFORMING_SUPPORT == 1)\r
1051         PHAL_DATA_TYPE  pHalData = GET_HAL_DATA(Adapter);\r
1052         PDM_ODM_T               pDM_Odm = &(pHalData->odmpriv);\r
1053 \r
1054         Beamforming_GetNDPAFrame(pDM_Odm, precv_frame);\r
1055 \r
1056 #else /*(BEAMFORMING_SUPPORT == 0)- for drv beamfoming*/\r
1057         u8      *ta;\r
1058         u8      idx, Sequence;\r
1059         u8      *pframe = precv_frame->u.hdr.rx_data;\r
1060         struct mlme_priv                        *pmlmepriv = &(Adapter->mlmepriv);\r
1061         struct beamforming_entry        *pBeamformEntry = NULL;\r
1062 \r
1063         /*DBG_871X("beamforming_get_ndpa_frame\n");*/\r
1064 \r
1065         if (IS_HARDWARE_TYPE_8812(Adapter) == _FALSE)\r
1066                 return;\r
1067         else if (GetFrameSubType(pframe) != WIFI_NDPA)\r
1068                 return;\r
1069 \r
1070         ta = GetAddr2Ptr(pframe);\r
1071         /*Remove signaling TA. */\r
1072         ta[0] = ta[0] & 0xFE; \r
1073         \r
1074         pBeamformEntry = beamforming_get_entry_by_addr(pmlmepriv, ta, &idx);\r
1075 \r
1076         if (pBeamformEntry == NULL)\r
1077                 return;\r
1078         else if (!(pBeamformEntry->beamforming_entry_cap & BEAMFORMEE_CAP_VHT_SU))\r
1079                 return;\r
1080         /*LogSuccess: As long as 8812A receive NDPA and feedback CSI succeed once, clock reset is NO LONGER needed !2015-04-10, Jeffery*/\r
1081         /*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*/\r
1082         else if ((pBeamformEntry->LogSuccess == 1) || (pBeamformEntry->ClockResetTimes == 5)) {\r
1083                 DBG_871X("[%s] LogSeq=%d, PreLogSeq=%d\n", __func__, pBeamformEntry->LogSeq, pBeamformEntry->PreLogSeq);\r
1084                 return;\r
1085         }\r
1086 \r
1087         Sequence = (pframe[16]) >> 2;\r
1088         DBG_871X("[%s] Start, Sequence=%d, LogSeq=%d, PreLogSeq=%d, LogRetryCnt=%d, ClockResetTimes=%d, LogSuccess=%d\n", \r
1089                 __func__, Sequence, pBeamformEntry->LogSeq, pBeamformEntry->PreLogSeq, pBeamformEntry->LogRetryCnt, pBeamformEntry->ClockResetTimes, pBeamformEntry->LogSuccess);\r
1090 \r
1091         if ((pBeamformEntry->LogSeq != 0) && (pBeamformEntry->PreLogSeq != 0)) {\r
1092                 /*Success condition*/\r
1093                 if ((pBeamformEntry->LogSeq != Sequence) && (pBeamformEntry->PreLogSeq != pBeamformEntry->LogSeq)) {\r
1094                         /* break option for clcok reset, 2015-03-30, Jeffery */\r
1095                         pBeamformEntry->LogRetryCnt = 0;\r
1096                         /*As long as 8812A receive NDPA and feedback CSI succeed once, clock reset is no longer needed.*/\r
1097                         /*That is, LogSuccess is NOT needed to be reset to zero, 2015-04-13, Jeffery*/\r
1098                         pBeamformEntry->LogSuccess = 1;\r
1099                 \r
1100                 } else {/*Fail condition*/\r
1101 \r
1102                         if (pBeamformEntry->LogRetryCnt == 5) {\r
1103                                 pBeamformEntry->ClockResetTimes++;\r
1104                 pBeamformEntry->LogRetryCnt = 0;\r
1105 \r
1106                                 DBG_871X("[%s] Clock Reset!!! ClockResetTimes=%d\n",  __func__, pBeamformEntry->ClockResetTimes);\r
1107                         beamforming_wk_cmd(Adapter, BEAMFORMING_CTRL_SOUNDING_CLK, NULL, 0, 1);\r
1108 \r
1109                         } else\r
1110                 pBeamformEntry->LogRetryCnt++;\r
1111         }\r
1112         }\r
1113 \r
1114         /*Update LogSeq & PreLogSeq*/\r
1115         pBeamformEntry->PreLogSeq = pBeamformEntry->LogSeq;\r
1116         pBeamformEntry->LogSeq = Sequence;\r
1117 \r
1118 #endif\r
1119         \r
1120 }\r
1121 \r
1122 \r
1123 \r
1124 \r
1125 void    beamforming_wk_hdl(_adapter *padapter, u8 type, u8 *pbuf)\r
1126 {\r
1127         PHAL_DATA_TYPE  pHalData = GET_HAL_DATA(padapter);\r
1128         PDM_ODM_T               pDM_Odm = &(pHalData->odmpriv);\r
1129 _func_enter_;\r
1130 \r
1131 #if (BEAMFORMING_SUPPORT == 1) /*(BEAMFORMING_SUPPORT == 1)- for PHYDM beamfoming*/\r
1132         switch (type) {\r
1133         case BEAMFORMING_CTRL_ENTER:\r
1134         {\r
1135                 struct sta_info *psta = (PVOID)pbuf;\r
1136                 u16                     staIdx = psta->mac_id;\r
1137 \r
1138                 Beamforming_Enter(pDM_Odm, staIdx);             \r
1139                 break;\r
1140         }\r
1141         case BEAMFORMING_CTRL_LEAVE:\r
1142                 Beamforming_Leave(pDM_Odm, pbuf);\r
1143                 break;\r
1144         default:\r
1145                 break;\r
1146 \r
1147         }\r
1148 #else /*(BEAMFORMING_SUPPORT == 0)- for drv beamfoming*/\r
1149         switch (type) { \r
1150                 case BEAMFORMING_CTRL_ENTER:\r
1151                         beamforming_enter(padapter, (PVOID)pbuf);\r
1152                         break;\r
1153 \r
1154                 case BEAMFORMING_CTRL_LEAVE:\r
1155                         beamforming_leave(padapter, pbuf);\r
1156                         break;\r
1157 \r
1158                 case BEAMFORMING_CTRL_SOUNDING_FAIL:\r
1159                         beamforming_sounding_fail(padapter);\r
1160                         break;\r
1161 \r
1162                 case BEAMFORMING_CTRL_SOUNDING_CLK:\r
1163                         rtw_hal_set_hwreg(padapter, HW_VAR_SOUNDING_CLK, NULL);\r
1164                         break;\r
1165         \r
1166                 default:\r
1167                         break;\r
1168         }\r
1169 #endif\r
1170 _func_exit_;\r
1171 }\r
1172 \r
1173 u8      beamforming_wk_cmd(_adapter*padapter, s32 type, u8 *pbuf, s32 size, u8 enqueue)\r
1174 {\r
1175         struct cmd_obj  *ph2c;\r
1176         struct drvextra_cmd_parm        *pdrvextra_cmd_parm;\r
1177         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;\r
1178         u8      res = _SUCCESS;\r
1179         \r
1180 _func_enter_;\r
1181 \r
1182         if(enqueue)\r
1183         {\r
1184                 u8      *wk_buf;\r
1185         \r
1186                 ph2c = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj));    \r
1187                 if(ph2c==NULL){\r
1188                         res= _FAIL;\r
1189                         goto exit;\r
1190                 }\r
1191                 \r
1192                 pdrvextra_cmd_parm = (struct drvextra_cmd_parm*)rtw_zmalloc(sizeof(struct drvextra_cmd_parm)); \r
1193                 if(pdrvextra_cmd_parm==NULL){\r
1194                         rtw_mfree((unsigned char *)ph2c, sizeof(struct cmd_obj));\r
1195                         res= _FAIL;\r
1196                         goto exit;\r
1197                 }\r
1198 \r
1199                 if (pbuf != NULL) {\r
1200                         wk_buf = rtw_zmalloc(size);\r
1201                         if(wk_buf==NULL){\r
1202                                 rtw_mfree((u8 *)ph2c, sizeof(struct cmd_obj));\r
1203                                 rtw_mfree((u8 *)pdrvextra_cmd_parm, sizeof(struct drvextra_cmd_parm));\r
1204                                 res= _FAIL;\r
1205                                 goto exit;\r
1206                         }\r
1207 \r
1208                         _rtw_memcpy(wk_buf, pbuf, size);\r
1209                 } else {\r
1210                         wk_buf = NULL;\r
1211                         size = 0;\r
1212                 }\r
1213 \r
1214                 pdrvextra_cmd_parm->ec_id = BEAMFORMING_WK_CID;\r
1215                 pdrvextra_cmd_parm->type = type;\r
1216                 pdrvextra_cmd_parm->size = size;\r
1217                 pdrvextra_cmd_parm->pbuf = wk_buf;\r
1218 \r
1219                 init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));\r
1220 \r
1221                 res = rtw_enqueue_cmd(pcmdpriv, ph2c);\r
1222         }\r
1223         else\r
1224         {\r
1225                 beamforming_wk_hdl(padapter, type, pbuf);\r
1226         }\r
1227         \r
1228 exit:\r
1229         \r
1230 _func_exit_;\r
1231 \r
1232         return res;\r
1233 }\r
1234 \r
1235 void update_attrib_txbf_info(_adapter *padapter, struct pkt_attrib *pattrib, struct sta_info *psta)\r
1236 {\r
1237         if (psta) {\r
1238                 pattrib->txbf_g_id = psta->txbf_gid;\r
1239                 pattrib->txbf_p_aid = psta->txbf_paid;\r
1240         }\r
1241 }\r
1242 \r
1243 #endif\r
1244 \r