drivers/staging/rtl8723bs/hal/rtl8723b_cmd.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 /******************************************************************************
   3  *
   4  * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
   5  *
   6  ******************************************************************************/
   7 #define _RTL8723B_CMD_C_
   8
   9 #include <drv_types.h>
  10 #include <rtw_debug.h>
  11 #include <rtl8723b_hal.h>
  12 #include "hal_com_h2c.h"
  13
  14 #define MAX_H2C_BOX_NUMS        4
  15 #define MESSAGE_BOX_SIZE        4
  16
  17 #define RTL8723B_MAX_CMD_LEN    7
  18 #define RTL8723B_EX_MESSAGE_BOX_SIZE    4
  19
  20 static u8 _is_fw_read_cmd_down(struct adapter *padapter, u8 msgbox_num)
  21 {
  22         u8 read_down = false;
  23         int retry_cnts = 100;
  24
  25         u8 valid;
  26
  27         do {
  28                 valid = rtw_read8(padapter, REG_HMETFR) & BIT(msgbox_num);
  29                 if (0 == valid) {
  30                         read_down = true;
  31                 }
  32         } while ((!read_down) && (retry_cnts--));
  33
  34         return read_down;
  35
  36 }
  37
  38
  39 /*****************************************
  40 * H2C Msg format :
  41 *| 31 - 8               |7-5    | 4 - 0 |
  42 *| h2c_msg      |Class  |CMD_ID |
  43 *| 31-0                                         |
  44 *| Ext msg                                      |
  45 *
  46 ******************************************/
  47 s32 FillH2CCmd8723B(struct adapter *padapter, u8 ElementID, u32 CmdLen, u8 *pCmdBuffer)
  48 {
  49         u8 h2c_box_num;
  50         u32 msgbox_addr;
  51         u32 msgbox_ex_addr = 0;
  52         struct hal_com_data *pHalData;
  53         u32 h2c_cmd = 0;
  54         u32 h2c_cmd_ex = 0;
  55         s32 ret = _FAIL;
  56
  57         padapter = GET_PRIMARY_ADAPTER(padapter);
  58         pHalData = GET_HAL_DATA(padapter);
  59         if (mutex_lock_interruptible(&(adapter_to_dvobj(padapter)->h2c_fwcmd_mutex)))
  60                 return ret;
  61
  62         if (!pCmdBuffer) {
  63                 goto exit;
  64         }
  65
  66         if (CmdLen > RTL8723B_MAX_CMD_LEN) {
  67                 goto exit;
  68         }
  69
  70         if (padapter->bSurpriseRemoved)
  71                 goto exit;
  72
  73         /* pay attention to if  race condition happened in  H2C cmd setting. */
  74         do {
  75                 h2c_box_num = pHalData->LastHMEBoxNum;
  76
  77                 if (!_is_fw_read_cmd_down(padapter, h2c_box_num))
  78                         goto exit;
  79
  80                 if (CmdLen <= 3)
  81                         memcpy((u8 *)(&h2c_cmd)+1, pCmdBuffer, CmdLen);
  82                 else {
  83                         memcpy((u8 *)(&h2c_cmd)+1, pCmdBuffer, 3);
  84                         memcpy((u8 *)(&h2c_cmd_ex), pCmdBuffer+3, CmdLen-3);
  85 /*                      *(u8 *)(&h2c_cmd) |= BIT(7); */
  86                 }
  87
  88                 *(u8 *)(&h2c_cmd) |= ElementID;
  89
  90                 if (CmdLen > 3) {
  91                         msgbox_ex_addr = REG_HMEBOX_EXT0_8723B + (h2c_box_num*RTL8723B_EX_MESSAGE_BOX_SIZE);
  92                         rtw_write32(padapter, msgbox_ex_addr, h2c_cmd_ex);
  93                 }
  94                 msgbox_addr = REG_HMEBOX_0 + (h2c_box_num*MESSAGE_BOX_SIZE);
  95                 rtw_write32(padapter, msgbox_addr, h2c_cmd);
  96
  97                 pHalData->LastHMEBoxNum = (h2c_box_num+1) % MAX_H2C_BOX_NUMS;
  98
  99         } while (0);
 100
 101         ret = _SUCCESS;
 102
 103 exit:
 104
 105         mutex_unlock(&(adapter_to_dvobj(padapter)->h2c_fwcmd_mutex));
 106         return ret;
 107 }
 108
 109 static void ConstructBeacon(struct adapter *padapter, u8 *pframe, u32 *pLength)
 110 {
 111         struct ieee80211_hdr *pwlanhdr;
 112         __le16 *fctrl;
 113         u32 rate_len, pktlen;
 114         struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
 115         struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
 116         struct wlan_bssid_ex *cur_network = &(pmlmeinfo->network);
 117         u8 bc_addr[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
 118
 119         pwlanhdr = (struct ieee80211_hdr *)pframe;
 120
 121         fctrl = &(pwlanhdr->frame_control);
 122         *(fctrl) = 0;
 123
 124         memcpy(pwlanhdr->addr1, bc_addr, ETH_ALEN);
 125         memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
 126         memcpy(pwlanhdr->addr3, get_my_bssid(cur_network), ETH_ALEN);
 127
 128         SetSeqNum(pwlanhdr, 0/*pmlmeext->mgnt_seq*/);
 129         /* pmlmeext->mgnt_seq++; */
 130         SetFrameSubType(pframe, WIFI_BEACON);
 131
 132         pframe += sizeof(struct ieee80211_hdr_3addr);
 133         pktlen = sizeof(struct ieee80211_hdr_3addr);
 134
 135         /* timestamp will be inserted by hardware */
 136         pframe += 8;
 137         pktlen += 8;
 138
 139         /*  beacon interval: 2 bytes */
 140         memcpy(pframe, (unsigned char *)(rtw_get_beacon_interval_from_ie(cur_network->IEs)), 2);
 141
 142         pframe += 2;
 143         pktlen += 2;
 144
 145         /*  capability info: 2 bytes */
 146         memcpy(pframe, (unsigned char *)(rtw_get_capability_from_ie(cur_network->IEs)), 2);
 147
 148         pframe += 2;
 149         pktlen += 2;
 150
 151         if ((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE) {
 152                 pktlen += cur_network->IELength - sizeof(struct ndis_802_11_fix_ie);
 153                 memcpy(pframe, cur_network->IEs+sizeof(struct ndis_802_11_fix_ie), pktlen);
 154
 155                 goto _ConstructBeacon;
 156         }
 157
 158         /* below for ad-hoc mode */
 159
 160         /*  SSID */
 161         pframe = rtw_set_ie(pframe, WLAN_EID_SSID, cur_network->Ssid.SsidLength, cur_network->Ssid.Ssid, &pktlen);
 162
 163         /*  supported rates... */
 164         rate_len = rtw_get_rateset_len(cur_network->SupportedRates);
 165         pframe = rtw_set_ie(pframe, WLAN_EID_SUPP_RATES, ((rate_len > 8) ? 8 : rate_len), cur_network->SupportedRates, &pktlen);
 166
 167         /*  DS parameter set */
 168         pframe = rtw_set_ie(pframe, WLAN_EID_DS_PARAMS, 1, (unsigned char *)&(cur_network->Configuration.DSConfig), &pktlen);
 169
 170         if ((pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE) {
 171                 u32 ATIMWindow;
 172                 /*  IBSS Parameter Set... */
 173                 /* ATIMWindow = cur->Configuration.ATIMWindow; */
 174                 ATIMWindow = 0;
 175                 pframe = rtw_set_ie(pframe, WLAN_EID_IBSS_PARAMS, 2, (unsigned char *)(&ATIMWindow), &pktlen);
 176         }
 177
 178
 179         /* todo: ERP IE */
 180
 181
 182         /*  EXTERNDED SUPPORTED RATE */
 183         if (rate_len > 8)
 184                 pframe = rtw_set_ie(pframe, WLAN_EID_EXT_SUPP_RATES, (rate_len - 8), (cur_network->SupportedRates + 8), &pktlen);
 185
 186
 187         /* todo:HT for adhoc */
 188
 189 _ConstructBeacon:
 190
 191         if ((pktlen + TXDESC_SIZE) > 512)
 192                 return;
 193
 194         *pLength = pktlen;
 195
 196 }
 197
 198 static void ConstructPSPoll(struct adapter *padapter, u8 *pframe, u32 *pLength)
 199 {
 200         struct ieee80211_hdr *pwlanhdr;
 201         __le16 *fctrl;
 202         struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
 203         struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
 204
 205         pwlanhdr = (struct ieee80211_hdr *)pframe;
 206
 207         /*  Frame control. */
 208         fctrl = &(pwlanhdr->frame_control);
 209         *(fctrl) = 0;
 210         SetPwrMgt(fctrl);
 211         SetFrameSubType(pframe, WIFI_PSPOLL);
 212
 213         /*  AID. */
 214         SetDuration(pframe, (pmlmeinfo->aid | 0xc000));
 215
 216         /*  BSSID. */
 217         memcpy(pwlanhdr->addr1, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
 218
 219         /*  TA. */
 220         memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
 221
 222         *pLength = 16;
 223 }
 224
 225 static void ConstructNullFunctionData(
 226         struct adapter *padapter,
 227         u8 *pframe,
 228         u32 *pLength,
 229         u8 *StaAddr,
 230         u8 bQoS,
 231         u8 AC,
 232         u8 bEosp,
 233         u8 bForcePowerSave
 234 )
 235 {
 236         struct ieee80211_hdr *pwlanhdr;
 237         __le16 *fctrl;
 238         u32 pktlen;
 239         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 240         struct wlan_network *cur_network = &pmlmepriv->cur_network;
 241         struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
 242         struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
 243
 244         pwlanhdr = (struct ieee80211_hdr *)pframe;
 245
 246         fctrl = &pwlanhdr->frame_control;
 247         *(fctrl) = 0;
 248         if (bForcePowerSave)
 249                 SetPwrMgt(fctrl);
 250
 251         switch (cur_network->network.InfrastructureMode) {
 252         case Ndis802_11Infrastructure:
 253                 SetToDs(fctrl);
 254                 memcpy(pwlanhdr->addr1, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
 255                 memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
 256                 memcpy(pwlanhdr->addr3, StaAddr, ETH_ALEN);
 257                 break;
 258         case Ndis802_11APMode:
 259                 SetFrDs(fctrl);
 260                 memcpy(pwlanhdr->addr1, StaAddr, ETH_ALEN);
 261                 memcpy(pwlanhdr->addr2, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
 262                 memcpy(pwlanhdr->addr3, myid(&(padapter->eeprompriv)), ETH_ALEN);
 263                 break;
 264         case Ndis802_11IBSS:
 265         default:
 266                 memcpy(pwlanhdr->addr1, StaAddr, ETH_ALEN);
 267                 memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
 268                 memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
 269                 break;
 270         }
 271
 272         SetSeqNum(pwlanhdr, 0);
 273
 274         if (bQoS) {
 275                 struct ieee80211_qos_hdr *pwlanqoshdr;
 276
 277                 SetFrameSubType(pframe, WIFI_QOS_DATA_NULL);
 278
 279                 pwlanqoshdr = (struct ieee80211_qos_hdr *)pframe;
 280                 SetPriority(&pwlanqoshdr->qos_ctrl, AC);
 281                 SetEOSP(&pwlanqoshdr->qos_ctrl, bEosp);
 282
 283                 pktlen = sizeof(struct ieee80211_qos_hdr);
 284         } else {
 285                 SetFrameSubType(pframe, WIFI_DATA_NULL);
 286
 287                 pktlen = sizeof(struct ieee80211_hdr_3addr);
 288         }
 289
 290         *pLength = pktlen;
 291 }
 292
 293 /*
 294  * To check if reserved page content is destroyed by beacon because beacon
 295  * is too large.
 296  */
 297 /* 2010.06.23. Added by tynli. */
 298 void CheckFwRsvdPageContent(struct adapter *Adapter)
 299 {
 300 }
 301
 302 static void rtl8723b_set_FwRsvdPage_cmd(struct adapter *padapter, struct rsvdpage_loc *rsvdpageloc)
 303 {
 304         u8 u1H2CRsvdPageParm[H2C_RSVDPAGE_LOC_LEN] = {0};
 305
 306         SET_8723B_H2CCMD_RSVDPAGE_LOC_PROBE_RSP(u1H2CRsvdPageParm, rsvdpageloc->LocProbeRsp);
 307         SET_8723B_H2CCMD_RSVDPAGE_LOC_PSPOLL(u1H2CRsvdPageParm, rsvdpageloc->LocPsPoll);
 308         SET_8723B_H2CCMD_RSVDPAGE_LOC_NULL_DATA(u1H2CRsvdPageParm, rsvdpageloc->LocNullData);
 309         SET_8723B_H2CCMD_RSVDPAGE_LOC_QOS_NULL_DATA(u1H2CRsvdPageParm, rsvdpageloc->LocQosNull);
 310         SET_8723B_H2CCMD_RSVDPAGE_LOC_BT_QOS_NULL_DATA(u1H2CRsvdPageParm, rsvdpageloc->LocBTQosNull);
 311
 312         print_hex_dump_debug(DRIVER_PREFIX ": u1H2CRsvdPageParm:", DUMP_PREFIX_NONE,
 313                              16, 1, u1H2CRsvdPageParm, H2C_RSVDPAGE_LOC_LEN, false);
 314
 315         FillH2CCmd8723B(padapter, H2C_8723B_RSVD_PAGE, H2C_RSVDPAGE_LOC_LEN, u1H2CRsvdPageParm);
 316 }
 317
 318 static void rtl8723b_set_FwAoacRsvdPage_cmd(struct adapter *padapter, struct rsvdpage_loc *rsvdpageloc)
 319 {
 320 }
 321
 322 void rtl8723b_set_FwMediaStatusRpt_cmd(struct adapter *padapter, u8 mstatus, u8 macid)
 323 {
 324         u8 u1H2CMediaStatusRptParm[H2C_MEDIA_STATUS_RPT_LEN] = {0};
 325         u8 macid_end = 0;
 326
 327         SET_8723B_H2CCMD_MSRRPT_PARM_OPMODE(u1H2CMediaStatusRptParm, mstatus);
 328         SET_8723B_H2CCMD_MSRRPT_PARM_MACID_IND(u1H2CMediaStatusRptParm, 0);
 329         SET_8723B_H2CCMD_MSRRPT_PARM_MACID(u1H2CMediaStatusRptParm, macid);
 330         SET_8723B_H2CCMD_MSRRPT_PARM_MACID_END(u1H2CMediaStatusRptParm, macid_end);
 331
 332         print_hex_dump_debug(DRIVER_PREFIX ": u1H2CMediaStatusRptParm:", DUMP_PREFIX_NONE,
 333                              16, 1, u1H2CMediaStatusRptParm, H2C_MEDIA_STATUS_RPT_LEN, false);
 334
 335         FillH2CCmd8723B(padapter, H2C_8723B_MEDIA_STATUS_RPT, H2C_MEDIA_STATUS_RPT_LEN, u1H2CMediaStatusRptParm);
 336 }
 337
 338 void rtl8723b_set_FwMacIdConfig_cmd(struct adapter *padapter, u8 mac_id, u8 raid, u8 bw, u8 sgi, u32 mask)
 339 {
 340         u8 u1H2CMacIdConfigParm[H2C_MACID_CFG_LEN] = {0};
 341
 342         SET_8723B_H2CCMD_MACID_CFG_MACID(u1H2CMacIdConfigParm, mac_id);
 343         SET_8723B_H2CCMD_MACID_CFG_RAID(u1H2CMacIdConfigParm, raid);
 344         SET_8723B_H2CCMD_MACID_CFG_SGI_EN(u1H2CMacIdConfigParm, sgi ? 1 : 0);
 345         SET_8723B_H2CCMD_MACID_CFG_BW(u1H2CMacIdConfigParm, bw);
 346         SET_8723B_H2CCMD_MACID_CFG_RATE_MASK0(u1H2CMacIdConfigParm, (u8)(mask & 0x000000ff));
 347         SET_8723B_H2CCMD_MACID_CFG_RATE_MASK1(u1H2CMacIdConfigParm, (u8)((mask & 0x0000ff00) >> 8));
 348         SET_8723B_H2CCMD_MACID_CFG_RATE_MASK2(u1H2CMacIdConfigParm, (u8)((mask & 0x00ff0000) >> 16));
 349         SET_8723B_H2CCMD_MACID_CFG_RATE_MASK3(u1H2CMacIdConfigParm, (u8)((mask & 0xff000000) >> 24));
 350
 351         print_hex_dump_debug(DRIVER_PREFIX ": u1H2CMacIdConfigParm:", DUMP_PREFIX_NONE,
 352                              16, 1, u1H2CMacIdConfigParm, H2C_MACID_CFG_LEN, false);
 353
 354         FillH2CCmd8723B(padapter, H2C_8723B_MACID_CFG, H2C_MACID_CFG_LEN, u1H2CMacIdConfigParm);
 355 }
 356
 357 void rtl8723b_set_rssi_cmd(struct adapter *padapter, u8 *param)
 358 {
 359         u8 u1H2CRssiSettingParm[H2C_RSSI_SETTING_LEN] = {0};
 360         u8 mac_id = *param;
 361         u8 rssi = *(param+2);
 362         u8 uldl_state = 0;
 363
 364         SET_8723B_H2CCMD_RSSI_SETTING_MACID(u1H2CRssiSettingParm, mac_id);
 365         SET_8723B_H2CCMD_RSSI_SETTING_RSSI(u1H2CRssiSettingParm, rssi);
 366         SET_8723B_H2CCMD_RSSI_SETTING_ULDL_STATE(u1H2CRssiSettingParm, uldl_state);
 367
 368         print_hex_dump_debug(DRIVER_PREFIX ": u1H2CRssiSettingParm:", DUMP_PREFIX_NONE,
 369                              16, 1, u1H2CRssiSettingParm, H2C_RSSI_SETTING_LEN, false);
 370
 371         FillH2CCmd8723B(padapter, H2C_8723B_RSSI_SETTING, H2C_RSSI_SETTING_LEN, u1H2CRssiSettingParm);
 372 }
 373
 374 void rtl8723b_set_FwPwrMode_cmd(struct adapter *padapter, u8 psmode)
 375 {
 376         int i;
 377         struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
 378         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 379         u8 u1H2CPwrModeParm[H2C_PWRMODE_LEN] = {0};
 380         u8 PowerState = 0, awake_intvl = 1, byte5 = 0, rlbm = 0;
 381
 382         if (pwrpriv->dtim > 0 && pwrpriv->dtim < 16)
 383                 awake_intvl = pwrpriv->dtim+1;/* DTIM = (awake_intvl - 1) */
 384         else
 385                 awake_intvl = 3;/* DTIM =2 */
 386
 387         rlbm = 2;
 388
 389         if (padapter->registrypriv.wifi_spec == 1) {
 390                 awake_intvl = 2;
 391                 rlbm = 2;
 392         }
 393
 394         if (psmode > 0) {
 395                 if (hal_btcoex_IsBtControlLps(padapter) == true) {
 396                         PowerState = hal_btcoex_RpwmVal(padapter);
 397                         byte5 = hal_btcoex_LpsVal(padapter);
 398
 399                         if ((rlbm == 2) && (byte5 & BIT(4))) {
 400                                 /*  Keep awake interval to 1 to prevent from */
 401                                 /*  decreasing coex performance */
 402                                 awake_intvl = 2;
 403                                 rlbm = 2;
 404                         }
 405                 } else {
 406                         PowerState = 0x00;/*  AllON(0x0C), RFON(0x04), RFOFF(0x00) */
 407                         byte5 = 0x40;
 408                 }
 409         } else {
 410                 PowerState = 0x0C;/*  AllON(0x0C), RFON(0x04), RFOFF(0x00) */
 411                 byte5 = 0x40;
 412         }
 413
 414         SET_8723B_H2CCMD_PWRMODE_PARM_MODE(u1H2CPwrModeParm, (psmode > 0) ? 1 : 0);
 415         SET_8723B_H2CCMD_PWRMODE_PARM_SMART_PS(u1H2CPwrModeParm, pwrpriv->smart_ps);
 416         SET_8723B_H2CCMD_PWRMODE_PARM_RLBM(u1H2CPwrModeParm, rlbm);
 417         SET_8723B_H2CCMD_PWRMODE_PARM_BCN_PASS_TIME(u1H2CPwrModeParm, awake_intvl);
 418         SET_8723B_H2CCMD_PWRMODE_PARM_ALL_QUEUE_UAPSD(u1H2CPwrModeParm, padapter->registrypriv.uapsd_enable);
 419         SET_8723B_H2CCMD_PWRMODE_PARM_PWR_STATE(u1H2CPwrModeParm, PowerState);
 420         SET_8723B_H2CCMD_PWRMODE_PARM_BYTE5(u1H2CPwrModeParm, byte5);
 421         if (psmode != PS_MODE_ACTIVE) {
 422                 if (!pmlmeext->adaptive_tsf_done && pmlmeext->bcn_cnt > 0) {
 423                         u8 ratio_20_delay, ratio_80_delay;
 424
 425                         /* byte 6 for adaptive_early_32k */
 426                         /* 0:3] = DrvBcnEarly  (ms) , [4:7] = DrvBcnTimeOut  (ms) */
 427                         /*  20% for DrvBcnEarly, 80% for DrvBcnTimeOut */
 428                         ratio_20_delay = 0;
 429                         ratio_80_delay = 0;
 430                         pmlmeext->DrvBcnEarly = 0xff;
 431                         pmlmeext->DrvBcnTimeOut = 0xff;
 432
 433                         for (i = 0; i < 9; i++) {
 434                                 pmlmeext->bcn_delay_ratio[i] = (pmlmeext->bcn_delay_cnt[i]*100)/pmlmeext->bcn_cnt;
 435
 436                                 ratio_20_delay += pmlmeext->bcn_delay_ratio[i];
 437                                 ratio_80_delay += pmlmeext->bcn_delay_ratio[i];
 438
 439                                 if (ratio_20_delay > 20 && pmlmeext->DrvBcnEarly == 0xff)
 440                                         pmlmeext->DrvBcnEarly = i;
 441
 442                                 if (ratio_80_delay > 80 && pmlmeext->DrvBcnTimeOut == 0xff)
 443                                         pmlmeext->DrvBcnTimeOut = i;
 444
 445                                 /* reset adaptive_early_32k cnt */
 446                                 pmlmeext->bcn_delay_cnt[i] = 0;
 447                                 pmlmeext->bcn_delay_ratio[i] = 0;
 448
 449                         }
 450
 451                         pmlmeext->bcn_cnt = 0;
 452                         pmlmeext->adaptive_tsf_done = true;
 453
 454                 }
 455
 456 /* offload to FW if fw version > v15.10
 457                 pmlmeext->DrvBcnEarly = 0;
 458                 pmlmeext->DrvBcnTimeOut =7;
 459
 460                 if ((pmlmeext->DrvBcnEarly!= 0Xff) && (pmlmeext->DrvBcnTimeOut!= 0xff))
 461                         u1H2CPwrModeParm[H2C_PWRMODE_LEN-1] = BIT(0) | ((pmlmeext->DrvBcnEarly<<1)&0x0E) |((pmlmeext->DrvBcnTimeOut<<4)&0xf0) ;
 462 */
 463
 464         }
 465
 466         hal_btcoex_RecordPwrMode(padapter, u1H2CPwrModeParm, H2C_PWRMODE_LEN);
 467
 468         print_hex_dump_debug(DRIVER_PREFIX ": u1H2CPwrModeParm:", DUMP_PREFIX_NONE,
 469                              16, 1, u1H2CPwrModeParm, H2C_PWRMODE_LEN, false);
 470
 471         FillH2CCmd8723B(padapter, H2C_8723B_SET_PWR_MODE, H2C_PWRMODE_LEN, u1H2CPwrModeParm);
 472 }
 473
 474 void rtl8723b_set_FwPsTuneParam_cmd(struct adapter *padapter)
 475 {
 476         u8 u1H2CPsTuneParm[H2C_PSTUNEPARAM_LEN] = {0};
 477         u8 bcn_to_limit = 10; /* 10 * 100 * awakeinterval (ms) */
 478         u8 dtim_timeout = 5; /* ms wait broadcast data timer */
 479         u8 ps_timeout = 20;  /* ms Keep awake when tx */
 480         u8 dtim_period = 3;
 481
 482         SET_8723B_H2CCMD_PSTUNE_PARM_BCN_TO_LIMIT(u1H2CPsTuneParm, bcn_to_limit);
 483         SET_8723B_H2CCMD_PSTUNE_PARM_DTIM_TIMEOUT(u1H2CPsTuneParm, dtim_timeout);
 484         SET_8723B_H2CCMD_PSTUNE_PARM_PS_TIMEOUT(u1H2CPsTuneParm, ps_timeout);
 485         SET_8723B_H2CCMD_PSTUNE_PARM_ADOPT(u1H2CPsTuneParm, 1);
 486         SET_8723B_H2CCMD_PSTUNE_PARM_DTIM_PERIOD(u1H2CPsTuneParm, dtim_period);
 487
 488         print_hex_dump_debug(DRIVER_PREFIX ": u1H2CPsTuneParm:", DUMP_PREFIX_NONE,
 489                              16, 1, u1H2CPsTuneParm, H2C_PSTUNEPARAM_LEN, false);
 490
 491         FillH2CCmd8723B(padapter, H2C_8723B_PS_TUNING_PARA, H2C_PSTUNEPARAM_LEN, u1H2CPsTuneParm);
 492 }
 493
 494 void rtl8723b_set_FwPwrModeInIPS_cmd(struct adapter *padapter, u8 cmd_param)
 495 {
 496
 497         FillH2CCmd8723B(padapter, H2C_8723B_FWLPS_IN_IPS_, 1, &cmd_param);
 498 }
 499
 500 /*
 501  * Description: Fill the reserved packets that FW will use to RSVD page.
 502  * Now we just send 4 types packet to rsvd page.
 503  * (1)Beacon, (2)Ps-poll, (3)Null data, (4)ProbeRsp.
 504  *
 505  * Input:
 506  *
 507  * bDLFinished - false: At the first time we will send all the packets as
 508  * a large packet to Hw, so we need to set the packet length to total length.
 509  *
 510  * true: At the second time, we should send the first packet (default:beacon)
 511  * to Hw again and set the length in descriptor to the real beacon length.
 512  */
 513 /* 2009.10.15 by tynli. */
 514 static void rtl8723b_set_FwRsvdPagePkt(
 515         struct adapter *padapter, bool bDLFinished
 516 )
 517 {
 518         struct xmit_frame *pcmdframe;
 519         struct pkt_attrib *pattrib;
 520         struct xmit_priv *pxmitpriv;
 521         struct mlme_ext_priv *pmlmeext;
 522         struct mlme_ext_info *pmlmeinfo;
 523         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 524         u32 BeaconLength = 0, PSPollLength = 0;
 525         u32 NullDataLength = 0, QosNullLength = 0, BTQosNullLength = 0;
 526         u8 *ReservedPagePacket;
 527         u8 TxDescLen = TXDESC_SIZE, TxDescOffset = TXDESC_OFFSET;
 528         u8 TotalPageNum = 0, CurtPktPageNum = 0, RsvdPageNum = 0;
 529         u16 BufIndex, PageSize = 128;
 530         u32 TotalPacketLen, MaxRsvdPageBufSize = 0;
 531
 532         struct rsvdpage_loc RsvdPageLoc;
 533
 534         pxmitpriv = &padapter->xmitpriv;
 535         pmlmeext = &padapter->mlmeextpriv;
 536         pmlmeinfo = &pmlmeext->mlmext_info;
 537
 538         RsvdPageNum = BCNQ_PAGE_NUM_8723B + WOWLAN_PAGE_NUM_8723B;
 539         MaxRsvdPageBufSize = RsvdPageNum*PageSize;
 540
 541         pcmdframe = rtw_alloc_cmdxmitframe(pxmitpriv);
 542         if (!pcmdframe)
 543                 return;
 544
 545         ReservedPagePacket = pcmdframe->buf_addr;
 546         memset(&RsvdPageLoc, 0, sizeof(struct rsvdpage_loc));
 547
 548         /* 3 (1) beacon */
 549         BufIndex = TxDescOffset;
 550         ConstructBeacon(padapter, &ReservedPagePacket[BufIndex], &BeaconLength);
 551
 552         /*  When we count the first page size, we need to reserve description size for the RSVD */
 553         /*  packet, it will be filled in front of the packet in TXPKTBUF. */
 554         CurtPktPageNum = (u8)PageNum_128(TxDescLen + BeaconLength);
 555         /* If we don't add 1 more page, the WOWLAN function has a problem. Baron thinks it's a bug of firmware */
 556         if (CurtPktPageNum == 1)
 557                 CurtPktPageNum += 1;
 558
 559         TotalPageNum += CurtPktPageNum;
 560
 561         BufIndex += (CurtPktPageNum*PageSize);
 562
 563         /* 3 (2) ps-poll */
 564         RsvdPageLoc.LocPsPoll = TotalPageNum;
 565         ConstructPSPoll(padapter, &ReservedPagePacket[BufIndex], &PSPollLength);
 566         rtl8723b_fill_fake_txdesc(padapter, &ReservedPagePacket[BufIndex-TxDescLen], PSPollLength, true, false, false);
 567
 568         CurtPktPageNum = (u8)PageNum_128(TxDescLen + PSPollLength);
 569
 570         TotalPageNum += CurtPktPageNum;
 571
 572         BufIndex += (CurtPktPageNum*PageSize);
 573
 574         /* 3 (3) null data */
 575         RsvdPageLoc.LocNullData = TotalPageNum;
 576         ConstructNullFunctionData(
 577                 padapter,
 578                 &ReservedPagePacket[BufIndex],
 579                 &NullDataLength,
 580                 get_my_bssid(&pmlmeinfo->network),
 581                 false, 0, 0, false
 582         );
 583         rtl8723b_fill_fake_txdesc(padapter, &ReservedPagePacket[BufIndex-TxDescLen], NullDataLength, false, false, false);
 584
 585         CurtPktPageNum = (u8)PageNum_128(TxDescLen + NullDataLength);
 586
 587         TotalPageNum += CurtPktPageNum;
 588
 589         BufIndex += (CurtPktPageNum*PageSize);
 590
 591         /* 3 (5) Qos null data */
 592         RsvdPageLoc.LocQosNull = TotalPageNum;
 593         ConstructNullFunctionData(
 594                 padapter,
 595                 &ReservedPagePacket[BufIndex],
 596                 &QosNullLength,
 597                 get_my_bssid(&pmlmeinfo->network),
 598                 true, 0, 0, false
 599         );
 600         rtl8723b_fill_fake_txdesc(padapter, &ReservedPagePacket[BufIndex-TxDescLen], QosNullLength, false, false, false);
 601
 602         CurtPktPageNum = (u8)PageNum_128(TxDescLen + QosNullLength);
 603
 604         TotalPageNum += CurtPktPageNum;
 605
 606         BufIndex += (CurtPktPageNum*PageSize);
 607
 608         /* 3 (6) BT Qos null data */
 609         RsvdPageLoc.LocBTQosNull = TotalPageNum;
 610         ConstructNullFunctionData(
 611                 padapter,
 612                 &ReservedPagePacket[BufIndex],
 613                 &BTQosNullLength,
 614                 get_my_bssid(&pmlmeinfo->network),
 615                 true, 0, 0, false
 616         );
 617         rtl8723b_fill_fake_txdesc(padapter, &ReservedPagePacket[BufIndex-TxDescLen], BTQosNullLength, false, true, false);
 618
 619         CurtPktPageNum = (u8)PageNum_128(TxDescLen + BTQosNullLength);
 620
 621         TotalPageNum += CurtPktPageNum;
 622
 623         BufIndex += (CurtPktPageNum*PageSize);
 624
 625         TotalPacketLen = BufIndex + BTQosNullLength;
 626
 627         if (TotalPacketLen > MaxRsvdPageBufSize) {
 628                 goto error;
 629         } else {
 630                 /*  update attribute */
 631                 pattrib = &pcmdframe->attrib;
 632                 update_mgntframe_attrib(padapter, pattrib);
 633                 pattrib->qsel = 0x10;
 634                 pattrib->pktlen = pattrib->last_txcmdsz = TotalPacketLen - TxDescOffset;
 635                 dump_mgntframe_and_wait(padapter, pcmdframe, 100);
 636         }
 637
 638         if (check_fwstate(pmlmepriv, _FW_LINKED)) {
 639                 rtl8723b_set_FwRsvdPage_cmd(padapter, &RsvdPageLoc);
 640                 rtl8723b_set_FwAoacRsvdPage_cmd(padapter, &RsvdPageLoc);
 641         } else {
 642                 rtl8723b_set_FwAoacRsvdPage_cmd(padapter, &RsvdPageLoc);
 643         }
 644         return;
 645
 646 error:
 647
 648         rtw_free_xmitframe(pxmitpriv, pcmdframe);
 649 }
 650
 651 void rtl8723b_download_rsvd_page(struct adapter *padapter, u8 mstatus)
 652 {
 653         struct hal_com_data     *pHalData = GET_HAL_DATA(padapter);
 654         struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
 655         struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
 656         bool bcn_valid = false;
 657         u8 DLBcnCount = 0;
 658         u32 poll = 0;
 659         u8 val8;
 660
 661         if (mstatus == RT_MEDIA_CONNECT) {
 662                 bool bRecover = false;
 663                 u8 v8;
 664
 665                 /*  We should set AID, correct TSF, HW seq enable before set JoinBssReport to Fw in 88/92C. */
 666                 /*  Suggested by filen. Added by tynli. */
 667                 rtw_write16(padapter, REG_BCN_PSR_RPT, (0xC000|pmlmeinfo->aid));
 668
 669                 /*  set REG_CR bit 8 */
 670                 v8 = rtw_read8(padapter, REG_CR+1);
 671                 v8 |= BIT(0); /*  ENSWBCN */
 672                 rtw_write8(padapter, REG_CR+1, v8);
 673
 674                 /*  Disable Hw protection for a time which revserd for Hw sending beacon. */
 675                 /*  Fix download reserved page packet fail that access collision with the protection time. */
 676                 /*  2010.05.11. Added by tynli. */
 677                 val8 = rtw_read8(padapter, REG_BCN_CTRL);
 678                 val8 &= ~EN_BCN_FUNCTION;
 679                 val8 |= DIS_TSF_UDT;
 680                 rtw_write8(padapter, REG_BCN_CTRL, val8);
 681
 682                 /*  Set FWHW_TXQ_CTRL 0x422[6]= 0 to tell Hw the packet is not a real beacon frame. */
 683                 if (pHalData->RegFwHwTxQCtrl & BIT(6))
 684                         bRecover = true;
 685
 686                 /*  To tell Hw the packet is not a real beacon frame. */
 687                 rtw_write8(padapter, REG_FWHW_TXQ_CTRL+2, pHalData->RegFwHwTxQCtrl & ~BIT(6));
 688                 pHalData->RegFwHwTxQCtrl &= ~BIT(6);
 689
 690                 /*  Clear beacon valid check bit. */
 691                 rtw_hal_set_hwreg(padapter, HW_VAR_BCN_VALID, NULL);
 692                 rtw_hal_set_hwreg(padapter, HW_VAR_DL_BCN_SEL, NULL);
 693
 694                 DLBcnCount = 0;
 695                 poll = 0;
 696                 do {
 697                         /*  download rsvd page. */
 698                         rtl8723b_set_FwRsvdPagePkt(padapter, 0);
 699                         DLBcnCount++;
 700                         do {
 701                                 yield();
 702                                 /* mdelay(10); */
 703                                 /*  check rsvd page download OK. */
 704                                 rtw_hal_get_hwreg(padapter, HW_VAR_BCN_VALID, (u8 *)(&bcn_valid));
 705                                 poll++;
 706                         } while (!bcn_valid && (poll%10) != 0 && !padapter->bSurpriseRemoved && !padapter->bDriverStopped);
 707
 708                 } while (!bcn_valid && DLBcnCount <= 100 && !padapter->bSurpriseRemoved && !padapter->bDriverStopped);
 709
 710                 if (padapter->bSurpriseRemoved || padapter->bDriverStopped) {
 711                 } else {
 712                         struct pwrctrl_priv *pwrctl = adapter_to_pwrctl(padapter);
 713                         pwrctl->fw_psmode_iface_id = padapter->iface_id;
 714                 }
 715
 716                 /*  2010.05.11. Added by tynli. */
 717                 val8 = rtw_read8(padapter, REG_BCN_CTRL);
 718                 val8 |= EN_BCN_FUNCTION;
 719                 val8 &= ~DIS_TSF_UDT;
 720                 rtw_write8(padapter, REG_BCN_CTRL, val8);
 721
 722                 /*  To make sure that if there exists an adapter which would like to send beacon. */
 723                 /*  If exists, the origianl value of 0x422[6] will be 1, we should check this to */
 724                 /*  prevent from setting 0x422[6] to 0 after download reserved page, or it will cause */
 725                 /*  the beacon cannot be sent by HW. */
 726                 /*  2010.06.23. Added by tynli. */
 727                 if (bRecover) {
 728                         rtw_write8(padapter, REG_FWHW_TXQ_CTRL+2, pHalData->RegFwHwTxQCtrl | BIT(6));
 729                         pHalData->RegFwHwTxQCtrl |= BIT(6);
 730                 }
 731
 732                 /*  Clear CR[8] or beacon packet will not be send to TxBuf anymore. */
 733                 v8 = rtw_read8(padapter, REG_CR+1);
 734                 v8 &= ~BIT(0); /*  ~ENSWBCN */
 735                 rtw_write8(padapter, REG_CR+1, v8);
 736         }
 737 }
 738
 739 void rtl8723b_set_FwJoinBssRpt_cmd(struct adapter *padapter, u8 mstatus)
 740 {
 741         if (mstatus == 1)
 742                 rtl8723b_download_rsvd_page(padapter, RT_MEDIA_CONNECT);
 743 }
 744
 745 /* arg[0] = macid */
 746 /* arg[1] = raid */
 747 /* arg[2] = shortGIrate */
 748 /* arg[3] = init_rate */
 749 void rtl8723b_Add_RateATid(
 750         struct adapter *padapter,
 751         u32 bitmap,
 752         u8 *arg,
 753         u8 rssi_level
 754 )
 755 {
 756         struct hal_com_data     *pHalData = GET_HAL_DATA(padapter);
 757         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 758         struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
 759         struct sta_info *psta;
 760         u8 mac_id = arg[0];
 761         u8 raid = arg[1];
 762         u8 shortGI = arg[2];
 763         u8 bw;
 764         u32 mask = bitmap&0x0FFFFFFF;
 765
 766         psta = pmlmeinfo->FW_sta_info[mac_id].psta;
 767         if (!psta)
 768                 return;
 769
 770         bw = psta->bw_mode;
 771
 772         if (rssi_level != DM_RATR_STA_INIT)
 773                 mask = ODM_Get_Rate_Bitmap(&pHalData->odmpriv, mac_id, mask, rssi_level);
 774
 775         rtl8723b_set_FwMacIdConfig_cmd(padapter, mac_id, raid, bw, shortGI, mask);
 776 }
 777
 778 static void ConstructBtNullFunctionData(
 779         struct adapter *padapter,
 780         u8 *pframe,
 781         u32 *pLength,
 782         u8 *StaAddr,
 783         u8 bQoS,
 784         u8 AC,
 785         u8 bEosp,
 786         u8 bForcePowerSave
 787 )
 788 {
 789         struct ieee80211_hdr *pwlanhdr;
 790         __le16 *fctrl;
 791         u32 pktlen;
 792         u8 bssid[ETH_ALEN];
 793
 794         pwlanhdr = (struct ieee80211_hdr *)pframe;
 795
 796         if (!StaAddr) {
 797                 memcpy(bssid, myid(&padapter->eeprompriv), ETH_ALEN);
 798                 StaAddr = bssid;
 799         }
 800
 801         fctrl = &pwlanhdr->frame_control;
 802         *fctrl = 0;
 803         if (bForcePowerSave)
 804                 SetPwrMgt(fctrl);
 805
 806         SetFrDs(fctrl);
 807         memcpy(pwlanhdr->addr1, StaAddr, ETH_ALEN);
 808         memcpy(pwlanhdr->addr2, myid(&padapter->eeprompriv), ETH_ALEN);
 809         memcpy(pwlanhdr->addr3, myid(&padapter->eeprompriv), ETH_ALEN);
 810
 811         SetDuration(pwlanhdr, 0);
 812         SetSeqNum(pwlanhdr, 0);
 813
 814         if (bQoS) {
 815                 struct ieee80211_qos_hdr *pwlanqoshdr;
 816
 817                 SetFrameSubType(pframe, WIFI_QOS_DATA_NULL);
 818
 819                 pwlanqoshdr = (struct ieee80211_qos_hdr *)pframe;
 820                 SetPriority(&pwlanqoshdr->qos_ctrl, AC);
 821                 SetEOSP(&pwlanqoshdr->qos_ctrl, bEosp);
 822
 823                 pktlen = sizeof(struct ieee80211_qos_hdr);
 824         } else {
 825                 SetFrameSubType(pframe, WIFI_DATA_NULL);
 826
 827                 pktlen = sizeof(struct ieee80211_hdr_3addr);
 828         }
 829
 830         *pLength = pktlen;
 831 }
 832
 833 static void SetFwRsvdPagePkt_BTCoex(struct adapter *padapter)
 834 {
 835         struct xmit_frame *pcmdframe;
 836         struct pkt_attrib *pattrib;
 837         struct xmit_priv *pxmitpriv;
 838         u32 BeaconLength = 0;
 839         u32 BTQosNullLength = 0;
 840         u8 *ReservedPagePacket;
 841         u8 TxDescLen, TxDescOffset;
 842         u8 TotalPageNum = 0, CurtPktPageNum = 0, RsvdPageNum = 0;
 843         u16 BufIndex, PageSize;
 844         u32 TotalPacketLen, MaxRsvdPageBufSize = 0;
 845         struct rsvdpage_loc RsvdPageLoc;
 846
 847         pxmitpriv = &padapter->xmitpriv;
 848         TxDescLen = TXDESC_SIZE;
 849         TxDescOffset = TXDESC_OFFSET;
 850         PageSize = PAGE_SIZE_TX_8723B;
 851
 852         RsvdPageNum = BCNQ_PAGE_NUM_8723B;
 853         MaxRsvdPageBufSize = RsvdPageNum*PageSize;
 854
 855         pcmdframe = rtw_alloc_cmdxmitframe(pxmitpriv);
 856         if (!pcmdframe)
 857                 return;
 858
 859         ReservedPagePacket = pcmdframe->buf_addr;
 860         memset(&RsvdPageLoc, 0, sizeof(struct rsvdpage_loc));
 861
 862         /* 3 (1) beacon */
 863         BufIndex = TxDescOffset;
 864         ConstructBeacon(padapter, &ReservedPagePacket[BufIndex], &BeaconLength);
 865
 866         /*  When we count the first page size, we need to reserve description size for the RSVD */
 867         /*  packet, it will be filled in front of the packet in TXPKTBUF. */
 868         CurtPktPageNum = (u8)PageNum_128(TxDescLen + BeaconLength);
 869         /* If we don't add 1 more page, the WOWLAN function has a problem. Baron thinks it's a bug of firmware */
 870         if (CurtPktPageNum == 1)
 871                 CurtPktPageNum += 1;
 872         TotalPageNum += CurtPktPageNum;
 873
 874         BufIndex += (CurtPktPageNum*PageSize);
 875
 876         /*  Jump to lastest page */
 877         if (BufIndex < (MaxRsvdPageBufSize - PageSize)) {
 878                 BufIndex = TxDescOffset + (MaxRsvdPageBufSize - PageSize);
 879                 TotalPageNum = BCNQ_PAGE_NUM_8723B - 1;
 880         }
 881
 882         /* 3 (6) BT Qos null data */
 883         RsvdPageLoc.LocBTQosNull = TotalPageNum;
 884         ConstructBtNullFunctionData(
 885                 padapter,
 886                 &ReservedPagePacket[BufIndex],
 887                 &BTQosNullLength,
 888                 NULL,
 889                 true, 0, 0, false
 890         );
 891         rtl8723b_fill_fake_txdesc(padapter, &ReservedPagePacket[BufIndex-TxDescLen], BTQosNullLength, false, true, false);
 892
 893         CurtPktPageNum = (u8)PageNum_128(TxDescLen + BTQosNullLength);
 894
 895         TotalPageNum += CurtPktPageNum;
 896
 897         TotalPacketLen = BufIndex + BTQosNullLength;
 898         if (TotalPacketLen > MaxRsvdPageBufSize)
 899                 goto error;
 900
 901         /*  update attribute */
 902         pattrib = &pcmdframe->attrib;
 903         update_mgntframe_attrib(padapter, pattrib);
 904         pattrib->qsel = 0x10;
 905         pattrib->pktlen = pattrib->last_txcmdsz = TotalPacketLen - TxDescOffset;
 906         dump_mgntframe_and_wait(padapter, pcmdframe, 100);
 907
 908         rtl8723b_set_FwRsvdPage_cmd(padapter, &RsvdPageLoc);
 909         rtl8723b_set_FwAoacRsvdPage_cmd(padapter, &RsvdPageLoc);
 910
 911         return;
 912
 913 error:
 914         rtw_free_xmitframe(pxmitpriv, pcmdframe);
 915 }
 916
 917 void rtl8723b_download_BTCoex_AP_mode_rsvd_page(struct adapter *padapter)
 918 {
 919         struct hal_com_data *pHalData;
 920         struct mlme_ext_priv *pmlmeext;
 921         struct mlme_ext_info *pmlmeinfo;
 922         u8 bRecover = false;
 923         u8 bcn_valid = false;
 924         u8 DLBcnCount = 0;
 925         u32 poll = 0;
 926         u8 val8;
 927
 928         pHalData = GET_HAL_DATA(padapter);
 929         pmlmeext = &padapter->mlmeextpriv;
 930         pmlmeinfo = &pmlmeext->mlmext_info;
 931
 932         /*  We should set AID, correct TSF, HW seq enable before set JoinBssReport to Fw in 88/92C. */
 933         /*  Suggested by filen. Added by tynli. */
 934         rtw_write16(padapter, REG_BCN_PSR_RPT, (0xC000|pmlmeinfo->aid));
 935
 936         /*  set REG_CR bit 8 */
 937         val8 = rtw_read8(padapter, REG_CR+1);
 938         val8 |= BIT(0); /*  ENSWBCN */
 939         rtw_write8(padapter,  REG_CR+1, val8);
 940
 941         /*  Disable Hw protection for a time which revserd for Hw sending beacon. */
 942         /*  Fix download reserved page packet fail that access collision with the protection time. */
 943         /*  2010.05.11. Added by tynli. */
 944         val8 = rtw_read8(padapter, REG_BCN_CTRL);
 945         val8 &= ~EN_BCN_FUNCTION;
 946         val8 |= DIS_TSF_UDT;
 947         rtw_write8(padapter, REG_BCN_CTRL, val8);
 948
 949         /*  Set FWHW_TXQ_CTRL 0x422[6]= 0 to tell Hw the packet is not a real beacon frame. */
 950         if (pHalData->RegFwHwTxQCtrl & BIT(6))
 951                 bRecover = true;
 952
 953         /*  To tell Hw the packet is not a real beacon frame. */
 954         pHalData->RegFwHwTxQCtrl &= ~BIT(6);
 955         rtw_write8(padapter, REG_FWHW_TXQ_CTRL+2, pHalData->RegFwHwTxQCtrl);
 956
 957         /*  Clear beacon valid check bit. */
 958         rtw_hal_set_hwreg(padapter, HW_VAR_BCN_VALID, NULL);
 959         rtw_hal_set_hwreg(padapter, HW_VAR_DL_BCN_SEL, NULL);
 960
 961         DLBcnCount = 0;
 962         poll = 0;
 963         do {
 964                 SetFwRsvdPagePkt_BTCoex(padapter);
 965                 DLBcnCount++;
 966                 do {
 967                         yield();
 968 /*                      mdelay(10); */
 969                         /*  check rsvd page download OK. */
 970                         rtw_hal_get_hwreg(padapter, HW_VAR_BCN_VALID, &bcn_valid);
 971                         poll++;
 972                 } while (!bcn_valid && (poll%10) != 0 && !padapter->bSurpriseRemoved && !padapter->bDriverStopped);
 973         } while (!bcn_valid && (DLBcnCount <= 100) && !padapter->bSurpriseRemoved && !padapter->bDriverStopped);
 974
 975         if (bcn_valid) {
 976                 struct pwrctrl_priv *pwrctl = adapter_to_pwrctl(padapter);
 977                 pwrctl->fw_psmode_iface_id = padapter->iface_id;
 978         }
 979
 980         /*  2010.05.11. Added by tynli. */
 981         val8 = rtw_read8(padapter, REG_BCN_CTRL);
 982         val8 |= EN_BCN_FUNCTION;
 983         val8 &= ~DIS_TSF_UDT;
 984         rtw_write8(padapter, REG_BCN_CTRL, val8);
 985
 986         /*  To make sure that if there exists an adapter which would like to send beacon. */
 987         /*  If exists, the origianl value of 0x422[6] will be 1, we should check this to */
 988         /*  prevent from setting 0x422[6] to 0 after download reserved page, or it will cause */
 989         /*  the beacon cannot be sent by HW. */
 990         /*  2010.06.23. Added by tynli. */
 991         if (bRecover) {
 992                 pHalData->RegFwHwTxQCtrl |= BIT(6);
 993                 rtw_write8(padapter, REG_FWHW_TXQ_CTRL+2, pHalData->RegFwHwTxQCtrl);
 994         }
 995
 996         /*  Clear CR[8] or beacon packet will not be send to TxBuf anymore. */
 997         val8 = rtw_read8(padapter, REG_CR+1);
 998         val8 &= ~BIT(0); /*  ~ENSWBCN */
 999         rtw_write8(padapter, REG_CR+1, val8);
1000 }