drivers/net/wireless/realtek/rtlwifi/rtl8723be/fw.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 /* Copyright(c) 2009-2014  Realtek Corporation.*/
   3
   4 #include "../wifi.h"
   5 #include "../pci.h"
   6 #include "../base.h"
   7 #include "../core.h"
   8 #include "reg.h"
   9 #include "def.h"
  10 #include "fw.h"
  11 #include "../rtl8723com/fw_common.h"
  12
  13 static bool _rtl8723be_check_fw_read_last_h2c(struct ieee80211_hw *hw,
  14                                               u8 boxnum)
  15 {
  16         struct rtl_priv *rtlpriv = rtl_priv(hw);
  17         u8 val_hmetfr;
  18         bool result = false;
  19
  20         val_hmetfr = rtl_read_byte(rtlpriv, REG_HMETFR);
  21         if (((val_hmetfr >> boxnum) & BIT(0)) == 0)
  22                 result = true;
  23         return result;
  24 }
  25
  26 static void _rtl8723be_fill_h2c_command(struct ieee80211_hw *hw, u8 element_id,
  27                                         u32 cmd_len, u8 *p_cmdbuffer)
  28 {
  29         struct rtl_priv *rtlpriv = rtl_priv(hw);
  30         struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
  31         u8 boxnum;
  32         u16 box_reg = 0, box_extreg = 0;
  33         u8 u1b_tmp;
  34         bool isfw_read = false;
  35         u8 buf_index = 0;
  36         bool bwrite_sucess = false;
  37         u8 wait_h2c_limmit = 100;
  38         u8 wait_writeh2c_limmit = 100;
  39         u8 boxcontent[4], boxextcontent[4];
  40         u32 h2c_waitcounter = 0;
  41         unsigned long flag;
  42         u8 idx;
  43
  44         RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD, "come in\n");
  45
  46         while (true) {
  47                 spin_lock_irqsave(&rtlpriv->locks.h2c_lock, flag);
  48                 if (rtlhal->h2c_setinprogress) {
  49                         RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
  50                                  "H2C set in progress! Wait to set..element_id(%d).\n",
  51                                  element_id);
  52
  53                         while (rtlhal->h2c_setinprogress) {
  54                                 spin_unlock_irqrestore(&rtlpriv->locks.h2c_lock,
  55                                                        flag);
  56                                 h2c_waitcounter++;
  57                                 RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
  58                                          "Wait 100 us (%d times)...\n",
  59                                          h2c_waitcounter);
  60                                 udelay(100);
  61
  62                                 if (h2c_waitcounter > 1000)
  63                                         return;
  64                                 spin_lock_irqsave(&rtlpriv->locks.h2c_lock,
  65                                                   flag);
  66                         }
  67                         spin_unlock_irqrestore(&rtlpriv->locks.h2c_lock, flag);
  68                 } else {
  69                         rtlhal->h2c_setinprogress = true;
  70                         spin_unlock_irqrestore(&rtlpriv->locks.h2c_lock, flag);
  71                         break;
  72                 }
  73         }
  74
  75         while (!bwrite_sucess) {
  76                 wait_writeh2c_limmit--;
  77                 if (wait_writeh2c_limmit == 0) {
  78                         pr_err("Write H2C fail because no trigger for FW INT!\n");
  79                         break;
  80                 }
  81
  82                 boxnum = rtlhal->last_hmeboxnum;
  83                 switch (boxnum) {
  84                 case 0:
  85                         box_reg = REG_HMEBOX_0;
  86                         box_extreg = REG_HMEBOX_EXT_0;
  87                         break;
  88                 case 1:
  89                         box_reg = REG_HMEBOX_1;
  90                         box_extreg = REG_HMEBOX_EXT_1;
  91                         break;
  92                 case 2:
  93                         box_reg = REG_HMEBOX_2;
  94                         box_extreg = REG_HMEBOX_EXT_2;
  95                         break;
  96                 case 3:
  97                         box_reg = REG_HMEBOX_3;
  98                         box_extreg = REG_HMEBOX_EXT_3;
  99                         break;
 100                 default:
 101                         pr_err("switch case %#x not processed\n",
 102                                boxnum);
 103                         break;
 104                 }
 105
 106                 isfw_read = _rtl8723be_check_fw_read_last_h2c(hw, boxnum);
 107                 while (!isfw_read) {
 108                         wait_h2c_limmit--;
 109                         if (wait_h2c_limmit == 0) {
 110                                 RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
 111                                          "Waiting too long for FW read clear HMEBox(%d)!\n",
 112                                          boxnum);
 113                                 break;
 114                         }
 115
 116                         udelay(10);
 117
 118                         isfw_read = _rtl8723be_check_fw_read_last_h2c(hw,
 119                                                                 boxnum);
 120                         u1b_tmp = rtl_read_byte(rtlpriv, 0x130);
 121                         RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
 122                                  "Waiting for FW read clear HMEBox(%d)!!! 0x130 = %2x\n",
 123                                  boxnum, u1b_tmp);
 124                 }
 125
 126                 if (!isfw_read) {
 127                         RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
 128                                  "Write H2C register BOX[%d] fail!!!!! Fw do not read.\n",
 129                                  boxnum);
 130                         break;
 131                 }
 132
 133                 memset(boxcontent, 0, sizeof(boxcontent));
 134                 memset(boxextcontent, 0, sizeof(boxextcontent));
 135                 boxcontent[0] = element_id;
 136                 RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
 137                          "Write element_id box_reg(%4x) = %2x\n",
 138                           box_reg, element_id);
 139
 140                 switch (cmd_len) {
 141                 case 1:
 142                 case 2:
 143                 case 3:
 144                         /*boxcontent[0] &= ~(BIT(7));*/
 145                         memcpy((u8 *)(boxcontent) + 1,
 146                                p_cmdbuffer + buf_index, cmd_len);
 147
 148                         for (idx = 0; idx < 4; idx++) {
 149                                 rtl_write_byte(rtlpriv, box_reg + idx,
 150                                                boxcontent[idx]);
 151                         }
 152                         break;
 153                 case 4:
 154                 case 5:
 155                 case 6:
 156                 case 7:
 157                         /*boxcontent[0] |= (BIT(7));*/
 158                         memcpy((u8 *)(boxextcontent),
 159                                p_cmdbuffer + buf_index+3, cmd_len-3);
 160                         memcpy((u8 *)(boxcontent) + 1,
 161                                p_cmdbuffer + buf_index, 3);
 162
 163                         for (idx = 0; idx < 4; idx++) {
 164                                 rtl_write_byte(rtlpriv, box_extreg + idx,
 165                                                boxextcontent[idx]);
 166                         }
 167
 168                         for (idx = 0; idx < 4; idx++) {
 169                                 rtl_write_byte(rtlpriv, box_reg + idx,
 170                                                boxcontent[idx]);
 171                         }
 172                         break;
 173                 default:
 174                         pr_err("switch case %#x not processed\n",
 175                                cmd_len);
 176                         break;
 177                 }
 178
 179                 bwrite_sucess = true;
 180
 181                 rtlhal->last_hmeboxnum = boxnum + 1;
 182                 if (rtlhal->last_hmeboxnum == 4)
 183                         rtlhal->last_hmeboxnum = 0;
 184
 185                 RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
 186                          "pHalData->last_hmeboxnum  = %d\n",
 187                           rtlhal->last_hmeboxnum);
 188         }
 189
 190         spin_lock_irqsave(&rtlpriv->locks.h2c_lock, flag);
 191         rtlhal->h2c_setinprogress = false;
 192         spin_unlock_irqrestore(&rtlpriv->locks.h2c_lock, flag);
 193
 194         RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD, "go out\n");
 195 }
 196
 197 void rtl8723be_fill_h2c_cmd(struct ieee80211_hw *hw, u8 element_id,
 198                             u32 cmd_len, u8 *p_cmdbuffer)
 199 {
 200         struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
 201         u32 tmp_cmdbuf[2];
 202
 203         if (!rtlhal->fw_ready) {
 204                 WARN_ONCE(true,
 205                           "rtl8723be: error H2C cmd because of Fw download fail!!!\n");
 206                 return;
 207         }
 208
 209         memset(tmp_cmdbuf, 0, 8);
 210         memcpy(tmp_cmdbuf, p_cmdbuffer, cmd_len);
 211         _rtl8723be_fill_h2c_command(hw, element_id, cmd_len,
 212                                     (u8 *)&tmp_cmdbuf);
 213         return;
 214 }
 215
 216 void rtl8723be_set_fw_pwrmode_cmd(struct ieee80211_hw *hw, u8 mode)
 217 {
 218         struct rtl_priv *rtlpriv = rtl_priv(hw);
 219         u8 u1_h2c_set_pwrmode[H2C_PWEMODE_LENGTH] = { 0 };
 220         struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
 221         u8 rlbm, power_state = 0, byte5 = 0;
 222         u8 awake_intvl; /* DTIM = (awake_intvl - 1) */
 223         struct rtl_btc_ops *btc_ops = rtlpriv->btcoexist.btc_ops;
 224         bool bt_ctrl_lps = (rtlpriv->cfg->ops->get_btc_status() ?
 225                             btc_ops->btc_is_bt_ctrl_lps(rtlpriv) : false);
 226         bool bt_lps_on = (rtlpriv->cfg->ops->get_btc_status() ?
 227                           btc_ops->btc_is_bt_lps_on(rtlpriv) : false);
 228
 229         if (bt_ctrl_lps)
 230                 mode = (bt_lps_on ? FW_PS_MIN_MODE : FW_PS_ACTIVE_MODE);
 231
 232         RT_TRACE(rtlpriv, COMP_POWER, DBG_DMESG, "FW LPS mode = %d (coex:%d)\n",
 233                  mode, bt_ctrl_lps);
 234
 235         switch (mode) {
 236         case FW_PS_MIN_MODE:
 237                 rlbm = 0;
 238                 awake_intvl = 2;
 239                 break;
 240         case FW_PS_MAX_MODE:
 241                 rlbm = 1;
 242                 awake_intvl = 2;
 243                 break;
 244         case FW_PS_DTIM_MODE:
 245                 rlbm = 2;
 246                 awake_intvl = ppsc->reg_max_lps_awakeintvl;
 247                 /* hw->conf.ps_dtim_period or mac->vif->bss_conf.dtim_period
 248                  * is only used in swlps.
 249                  */
 250                 break;
 251         default:
 252                 rlbm = 2;
 253                 awake_intvl = 4;
 254                 break;
 255         }
 256
 257         if (rtlpriv->mac80211.p2p) {
 258                 awake_intvl = 2;
 259                 rlbm = 1;
 260         }
 261
 262         if (mode == FW_PS_ACTIVE_MODE) {
 263                 byte5 = 0x40;
 264                 power_state = FW_PWR_STATE_ACTIVE;
 265         } else {
 266                 if (bt_ctrl_lps) {
 267                         byte5 = btc_ops->btc_get_lps_val(rtlpriv);
 268                         power_state = btc_ops->btc_get_rpwm_val(rtlpriv);
 269
 270                         if ((rlbm == 2) && (byte5 & BIT(4))) {
 271                                 /* Keep awake interval to 1 to prevent from
 272                                  * decreasing coex performance
 273                                  */
 274                                 awake_intvl = 2;
 275                                 rlbm = 2;
 276                         }
 277                 } else {
 278                         byte5 = 0x40;
 279                         power_state = FW_PWR_STATE_RF_OFF;
 280                 }
 281         }
 282
 283         SET_H2CCMD_PWRMODE_PARM_MODE(u1_h2c_set_pwrmode, ((mode) ? 1 : 0));
 284         SET_H2CCMD_PWRMODE_PARM_RLBM(u1_h2c_set_pwrmode, rlbm);
 285         SET_H2CCMD_PWRMODE_PARM_SMART_PS(u1_h2c_set_pwrmode,
 286                                          bt_ctrl_lps ? 0 : ppsc->smart_ps);
 287         SET_H2CCMD_PWRMODE_PARM_AWAKE_INTERVAL(u1_h2c_set_pwrmode,
 288                                                awake_intvl);
 289         SET_H2CCMD_PWRMODE_PARM_ALL_QUEUE_UAPSD(u1_h2c_set_pwrmode, 0);
 290         SET_H2CCMD_PWRMODE_PARM_PWR_STATE(u1_h2c_set_pwrmode, power_state);
 291         SET_H2CCMD_PWRMODE_PARM_BYTE5(u1_h2c_set_pwrmode, byte5);
 292
 293         RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_DMESG,
 294                       "rtl92c_set_fw_pwrmode(): u1_h2c_set_pwrmode\n",
 295                       u1_h2c_set_pwrmode, H2C_PWEMODE_LENGTH);
 296         if (rtlpriv->cfg->ops->get_btc_status())
 297                 btc_ops->btc_record_pwr_mode(rtlpriv, u1_h2c_set_pwrmode,
 298                                              H2C_PWEMODE_LENGTH);
 299         rtl8723be_fill_h2c_cmd(hw, H2C_8723B_SETPWRMODE, H2C_PWEMODE_LENGTH,
 300                                u1_h2c_set_pwrmode);
 301 }
 302
 303 void rtl8723be_set_fw_media_status_rpt_cmd(struct ieee80211_hw *hw, u8 mstatus)
 304 {
 305         u8 parm[3] = { 0, 0, 0 };
 306         /* parm[0]: bit0=0-->Disconnect, bit0=1-->Connect
 307          *          bit1=0-->update Media Status to MACID
 308          *          bit1=1-->update Media Status from MACID to MACID_End
 309          * parm[1]: MACID, if this is INFRA_STA, MacID = 0
 310          * parm[2]: MACID_End
 311         */
 312         SET_H2CCMD_MSRRPT_PARM_OPMODE(parm, mstatus);
 313         SET_H2CCMD_MSRRPT_PARM_MACID_IND(parm, 0);
 314
 315         rtl8723be_fill_h2c_cmd(hw, H2C_8723B_MSRRPT, 3, parm);
 316 }
 317
 318 #define BEACON_PG               0 /* ->1 */
 319 #define PSPOLL_PG               2
 320 #define NULL_PG                 3
 321 #define PROBERSP_PG             4 /* ->5 */
 322 #define QOS_NULL_PG             6
 323 #define BT_QOS_NULL_PG  7
 324
 325 #define TOTAL_RESERVED_PKT_LEN  1024    /* can be up to 1280 (tx_bndy=245) */
 326
 327 static u8 reserved_page_packet[TOTAL_RESERVED_PKT_LEN] = {
 328         /* page 0 beacon */
 329         0x80, 0x00, 0x00, 0x00, 0xFF, 0xFF, 0xFF, 0xFF,
 330         0xFF, 0xFF, 0x00, 0xE0, 0x4C, 0x02, 0xB1, 0x78,
 331         0xEC, 0x1A, 0x59, 0x0B, 0xAD, 0xD4, 0x20, 0x00,
 332         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 333         0x64, 0x00, 0x10, 0x04, 0x00, 0x05, 0x54, 0x65,
 334         0x73, 0x74, 0x32, 0x01, 0x08, 0x82, 0x84, 0x0B,
 335         0x16, 0x24, 0x30, 0x48, 0x6C, 0x03, 0x01, 0x06,
 336         0x06, 0x02, 0x00, 0x00, 0x2A, 0x01, 0x02, 0x32,
 337         0x04, 0x0C, 0x12, 0x18, 0x60, 0x2D, 0x1A, 0x6C,
 338         0x09, 0x03, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
 339         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 340         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 341         0x00, 0x3D, 0x00, 0xDD, 0x07, 0x00, 0xE0, 0x4C,
 342         0x02, 0x02, 0x00, 0x00, 0xDD, 0x18, 0x00, 0x50,
 343         0xF2, 0x01, 0x01, 0x00, 0x00, 0x50, 0xF2, 0x04,
 344         0x01, 0x00, 0x00, 0x50, 0xF2, 0x04, 0x01, 0x00,
 345
 346         /* page 1 beacon */
 347         0x00, 0x50, 0xF2, 0x02, 0x00, 0x00, 0x00, 0x00,
 348         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 349         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 350         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 351         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 352         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 353         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 354         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 355         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 356         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 357         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 358         0x10, 0x00, 0x28, 0x8C, 0x00, 0x12, 0x00, 0x00,
 359         0x00, 0x00, 0x00, 0x00, 0x00, 0x81, 0x00, 0x00,
 360         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 361         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 362         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 363
 364         /* page 2  ps-poll */
 365         0xA4, 0x10, 0x01, 0xC0, 0xEC, 0x1A, 0x59, 0x0B,
 366         0xAD, 0xD4, 0x00, 0xE0, 0x4C, 0x02, 0xB1, 0x78,
 367         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 368         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 369         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 370         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 371         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 372         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 373         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 374         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 375         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 376         0x18, 0x00, 0x28, 0x8C, 0x00, 0x12, 0x00, 0x00,
 377         0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00,
 378         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 379         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 380         0x00, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 381
 382         /* page 3  null */
 383         0x48, 0x01, 0x00, 0x00, 0xEC, 0x1A, 0x59, 0x0B,
 384         0xAD, 0xD4, 0x00, 0xE0, 0x4C, 0x02, 0xB1, 0x78,
 385         0xEC, 0x1A, 0x59, 0x0B, 0xAD, 0xD4, 0x00, 0x00,
 386         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 387         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 388         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 389         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 390         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 391         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 392         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 393         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 394         0x72, 0x00, 0x28, 0x8C, 0x00, 0x12, 0x00, 0x00,
 395         0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00,
 396         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 397         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 398         0x00, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 399
 400         /* page 4  probe_resp */
 401         0x50, 0x00, 0x00, 0x00, 0x00, 0x40, 0x10, 0x10,
 402         0x00, 0x03, 0x00, 0xE0, 0x4C, 0x76, 0x00, 0x42,
 403         0x00, 0x40, 0x10, 0x10, 0x00, 0x03, 0x00, 0x00,
 404         0x9E, 0x46, 0x15, 0x32, 0x27, 0xF2, 0x2D, 0x00,
 405         0x64, 0x00, 0x00, 0x04, 0x00, 0x0C, 0x6C, 0x69,
 406         0x6E, 0x6B, 0x73, 0x79, 0x73, 0x5F, 0x77, 0x6C,
 407         0x61, 0x6E, 0x01, 0x04, 0x82, 0x84, 0x8B, 0x96,
 408         0x03, 0x01, 0x01, 0x06, 0x02, 0x00, 0x00, 0x2A,
 409         0x01, 0x00, 0x32, 0x08, 0x24, 0x30, 0x48, 0x6C,
 410         0x0C, 0x12, 0x18, 0x60, 0x2D, 0x1A, 0x6C, 0x18,
 411         0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 412         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 413         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 414         0x3D, 0x00, 0xDD, 0x06, 0x00, 0xE0, 0x4C, 0x02,
 415         0x01, 0x70, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 416         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 417
 418         /* page 5  probe_resp */
 419         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 420         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 421         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 422         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 423         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 424         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 425         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 426         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 427         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 428         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 429         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 430         0x1A, 0x00, 0x28, 0x8C, 0x00, 0x12, 0x00, 0x00,
 431         0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00,
 432         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 433         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 434         0x00, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 435
 436         /* page 6 qos null data */
 437         0xC8, 0x01, 0x00, 0x00, 0x84, 0xC9, 0xB2, 0xA7,
 438         0xB3, 0x6E, 0x00, 0xE0, 0x4C, 0x02, 0x51, 0x02,
 439         0x84, 0xC9, 0xB2, 0xA7, 0xB3, 0x6E, 0x00, 0x00,
 440         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 441         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 442         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 443         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 444         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 445         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 446         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 447         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 448         0x1A, 0x00, 0x28, 0x8C, 0x00, 0x12, 0x00, 0x00,
 449         0x00, 0x00, 0x80, 0x00, 0x00, 0x01, 0x00, 0x00,
 450         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 451         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 452         0x00, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 453
 454         /* page 7 BT-qos null data */
 455         0xC8, 0x01, 0x00, 0x00, 0x84, 0xC9, 0xB2, 0xA7,
 456         0xB3, 0x6E, 0x00, 0xE0, 0x4C, 0x02, 0x51, 0x02,
 457         0x84, 0xC9, 0xB2, 0xA7, 0xB3, 0x6E, 0x00, 0x00,
 458         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 459         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 460         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 461         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 462         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 463         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 464         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 465         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 466         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 467         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 468         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 469         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 470         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 471
 472 };
 473
 474 void rtl8723be_set_fw_rsvdpagepkt(struct ieee80211_hw *hw,
 475                                   bool b_dl_finished)
 476 {
 477         struct rtl_priv *rtlpriv = rtl_priv(hw);
 478         struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
 479         struct sk_buff *skb = NULL;
 480
 481         u32 totalpacketlen;
 482         bool rtstatus;
 483         u8 u1rsvdpageloc[5] = { 0 };
 484         bool b_dlok = false;
 485
 486         u8 *beacon;
 487         u8 *p_pspoll;
 488         u8 *nullfunc;
 489         u8 *p_probersp;
 490         u8 *qosnull;
 491         u8 *btqosnull;
 492         /*---------------------------------------------------------
 493          *                      (1) beacon
 494          *---------------------------------------------------------
 495          */
 496         beacon = &reserved_page_packet[BEACON_PG * 128];
 497         SET_80211_HDR_ADDRESS2(beacon, mac->mac_addr);
 498         SET_80211_HDR_ADDRESS3(beacon, mac->bssid);
 499
 500         /*-------------------------------------------------------
 501          *                      (2) ps-poll
 502          *-------------------------------------------------------
 503          */
 504         p_pspoll = &reserved_page_packet[PSPOLL_PG * 128];
 505         SET_80211_PS_POLL_AID(p_pspoll, (mac->assoc_id | 0xc000));
 506         SET_80211_PS_POLL_BSSID(p_pspoll, mac->bssid);
 507         SET_80211_PS_POLL_TA(p_pspoll, mac->mac_addr);
 508
 509         SET_H2CCMD_RSVDPAGE_LOC_PSPOLL(u1rsvdpageloc, PSPOLL_PG);
 510
 511         /*--------------------------------------------------------
 512          *                      (3) null data
 513          *--------------------------------------------------------
 514          */
 515         nullfunc = &reserved_page_packet[NULL_PG * 128];
 516         SET_80211_HDR_ADDRESS1(nullfunc, mac->bssid);
 517         SET_80211_HDR_ADDRESS2(nullfunc, mac->mac_addr);
 518         SET_80211_HDR_ADDRESS3(nullfunc, mac->bssid);
 519
 520         SET_H2CCMD_RSVDPAGE_LOC_NULL_DATA(u1rsvdpageloc, NULL_PG);
 521
 522         /*---------------------------------------------------------
 523          *                      (4) probe response
 524          *---------------------------------------------------------
 525          */
 526         p_probersp = &reserved_page_packet[PROBERSP_PG * 128];
 527         SET_80211_HDR_ADDRESS1(p_probersp, mac->bssid);
 528         SET_80211_HDR_ADDRESS2(p_probersp, mac->mac_addr);
 529         SET_80211_HDR_ADDRESS3(p_probersp, mac->bssid);
 530
 531         SET_H2CCMD_RSVDPAGE_LOC_PROBE_RSP(u1rsvdpageloc, PROBERSP_PG);
 532
 533         /*---------------------------------------------------------
 534          *                      (5) QoS Null
 535          *---------------------------------------------------------
 536          */
 537         qosnull = &reserved_page_packet[QOS_NULL_PG * 128];
 538         SET_80211_HDR_ADDRESS1(qosnull, mac->bssid);
 539         SET_80211_HDR_ADDRESS2(qosnull, mac->mac_addr);
 540         SET_80211_HDR_ADDRESS3(qosnull, mac->bssid);
 541
 542         SET_H2CCMD_RSVDPAGE_LOC_QOS_NULL_DATA(u1rsvdpageloc, QOS_NULL_PG);
 543
 544         /*---------------------------------------------------------
 545          *                      (5) QoS Null
 546          *---------------------------------------------------------
 547          */
 548         btqosnull = &reserved_page_packet[BT_QOS_NULL_PG * 128];
 549         SET_80211_HDR_ADDRESS1(btqosnull, mac->bssid);
 550         SET_80211_HDR_ADDRESS2(btqosnull, mac->mac_addr);
 551         SET_80211_HDR_ADDRESS3(btqosnull, mac->bssid);
 552
 553         SET_H2CCMD_RSVDPAGE_LOC_BT_QOS_NULL_DATA(u1rsvdpageloc, BT_QOS_NULL_PG);
 554
 555         totalpacketlen = TOTAL_RESERVED_PKT_LEN;
 556
 557         RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_LOUD,
 558                       "rtl8723be_set_fw_rsvdpagepkt(): HW_VAR_SET_TX_CMD: ALL\n",
 559                       &reserved_page_packet[0], totalpacketlen);
 560         RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_DMESG,
 561                       "rtl8723be_set_fw_rsvdpagepkt(): HW_VAR_SET_TX_CMD: ALL\n",
 562                       u1rsvdpageloc, sizeof(u1rsvdpageloc));
 563
 564         skb = dev_alloc_skb(totalpacketlen);
 565         if (!skb)
 566                 return;
 567         skb_put_data(skb, &reserved_page_packet, totalpacketlen);
 568
 569         rtstatus = rtl_cmd_send_packet(hw, skb);
 570
 571         if (rtstatus)
 572                 b_dlok = true;
 573
 574         if (b_dlok) {
 575                 RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD,
 576                          "Set RSVD page location to Fw.\n");
 577                 RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_DMESG, "H2C_RSVDPAGE:\n",
 578                               u1rsvdpageloc, sizeof(u1rsvdpageloc));
 579                 rtl8723be_fill_h2c_cmd(hw, H2C_8723B_RSVDPAGE,
 580                                        sizeof(u1rsvdpageloc), u1rsvdpageloc);
 581         } else
 582                 RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
 583                          "Set RSVD page location to Fw FAIL!!!!!!.\n");
 584 }
 585
 586 /*Should check FW support p2p or not.*/
 587 static void rtl8723be_set_p2p_ctw_period_cmd(struct ieee80211_hw *hw,
 588                                              u8 ctwindow)
 589 {
 590         u8 u1_ctwindow_period[1] = { ctwindow};
 591
 592         rtl8723be_fill_h2c_cmd(hw, H2C_8723B_P2P_PS_CTW_CMD, 1,
 593                                u1_ctwindow_period);
 594 }
 595
 596 void rtl8723be_set_p2p_ps_offload_cmd(struct ieee80211_hw *hw,
 597                                       u8 p2p_ps_state)
 598 {
 599         struct rtl_priv *rtlpriv = rtl_priv(hw);
 600         struct rtl_ps_ctl *rtlps = rtl_psc(rtl_priv(hw));
 601         struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
 602         struct rtl_p2p_ps_info *p2pinfo = &(rtlps->p2p_ps_info);
 603         struct p2p_ps_offload_t *p2p_ps_offload = &rtlhal->p2p_ps_offload;
 604         u8 i;
 605         u16 ctwindow;
 606         u32 start_time, tsf_low;
 607
 608         switch (p2p_ps_state) {
 609         case P2P_PS_DISABLE:
 610                 RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "P2P_PS_DISABLE\n");
 611                 memset(p2p_ps_offload, 0, sizeof(*p2p_ps_offload));
 612                 break;
 613         case P2P_PS_ENABLE:
 614                 RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "P2P_PS_ENABLE\n");
 615                 /* update CTWindow value. */
 616                 if (p2pinfo->ctwindow > 0) {
 617                         p2p_ps_offload->ctwindow_en = 1;
 618                         ctwindow = p2pinfo->ctwindow;
 619                         rtl8723be_set_p2p_ctw_period_cmd(hw, ctwindow);
 620                 }
 621                 /* hw only support 2 set of NoA */
 622                 for (i = 0 ; i < p2pinfo->noa_num ; i++) {
 623                         /* To control the register setting
 624                          * for which NOA
 625                          */
 626                         rtl_write_byte(rtlpriv, 0x5cf, (i << 4));
 627                         if (i == 0)
 628                                 p2p_ps_offload->noa0_en = 1;
 629                         else
 630                                 p2p_ps_offload->noa1_en = 1;
 631
 632                         /* config P2P NoA Descriptor Register */
 633                         rtl_write_dword(rtlpriv, 0x5E0,
 634                                         p2pinfo->noa_duration[i]);
 635                         rtl_write_dword(rtlpriv, 0x5E4,
 636                                         p2pinfo->noa_interval[i]);
 637
 638                         /*Get Current TSF value */
 639                         tsf_low = rtl_read_dword(rtlpriv, REG_TSFTR);
 640
 641                         start_time = p2pinfo->noa_start_time[i];
 642                         if (p2pinfo->noa_count_type[i] != 1) {
 643                                 while (start_time <= (tsf_low + (50 * 1024))) {
 644                                         start_time += p2pinfo->noa_interval[i];
 645                                         if (p2pinfo->noa_count_type[i] != 255)
 646                                                 p2pinfo->noa_count_type[i]--;
 647                                 }
 648                         }
 649                         rtl_write_dword(rtlpriv, 0x5E8, start_time);
 650                         rtl_write_dword(rtlpriv, 0x5EC,
 651                                         p2pinfo->noa_count_type[i]);
 652                 }
 653
 654                 if ((p2pinfo->opp_ps == 1) ||
 655                     (p2pinfo->noa_num > 0)) {
 656                         /* rst p2p circuit */
 657                         rtl_write_byte(rtlpriv, REG_DUAL_TSF_RST, BIT(4));
 658
 659                         p2p_ps_offload->offload_en = 1;
 660
 661                         if (P2P_ROLE_GO == rtlpriv->mac80211.p2p) {
 662                                 p2p_ps_offload->role = 1;
 663                                 p2p_ps_offload->allstasleep = 0;
 664                         } else {
 665                                 p2p_ps_offload->role = 0;
 666                         }
 667                         p2p_ps_offload->discovery = 0;
 668                 }
 669                 break;
 670         case P2P_PS_SCAN:
 671                 RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "P2P_PS_SCAN\n");
 672                 p2p_ps_offload->discovery = 1;
 673                 break;
 674         case P2P_PS_SCAN_DONE:
 675                 RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "P2P_PS_SCAN_DONE\n");
 676                 p2p_ps_offload->discovery = 0;
 677                 p2pinfo->p2p_ps_state = P2P_PS_ENABLE;
 678                 break;
 679         default:
 680                 break;
 681         }
 682
 683         rtl8723be_fill_h2c_cmd(hw, H2C_8723B_P2P_PS_OFFLOAD, 1,
 684                                (u8 *)p2p_ps_offload);
 685 }