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

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