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 "reg.h"
  30 #include "def.h"
  31 #include "fw.h"
  32 #include "../rtl8723com/fw_common.h"
  33
  34 static bool _rtl8723be_check_fw_read_last_h2c(struct ieee80211_hw *hw,
  35                                               u8 boxnum)
  36 {
  37         struct rtl_priv *rtlpriv = rtl_priv(hw);
  38         u8 val_hmetfr;
  39         bool result = false;
  40
  41         val_hmetfr = rtl_read_byte(rtlpriv, REG_HMETFR);
  42         if (((val_hmetfr >> boxnum) & BIT(0)) == 0)
  43                 result = true;
  44         return result;
  45 }
  46
  47 static void _rtl8723be_fill_h2c_command(struct ieee80211_hw *hw, u8 element_id,
  48                                         u32 cmd_len, u8 *p_cmdbuffer)
  49 {
  50         struct rtl_priv *rtlpriv = rtl_priv(hw);
  51         struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
  52         u8 boxnum;
  53         u16 box_reg = 0, box_extreg = 0;
  54         u8 u1b_tmp;
  55         bool isfw_read = false;
  56         u8 buf_index = 0;
  57         bool bwrite_sucess = false;
  58         u8 wait_h2c_limit = 100;
  59         u8 wait_writeh2c_limit = 100;
  60         u8 boxcontent[4], boxextcontent[4];
  61         u32 h2c_waitcounter = 0;
  62         unsigned long flag;
  63         u8 idx;
  64
  65         RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD, "come in\n");
  66
  67         while (true) {
  68                 spin_lock_irqsave(&rtlpriv->locks.h2c_lock, flag);
  69                 if (rtlhal->h2c_setinprogress) {
  70                         RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
  71                                  "H2C set in progress! Wait to set.."
  72                                  "element_id(%d).\n", element_id);
  73
  74                         while (rtlhal->h2c_setinprogress) {
  75                                 spin_unlock_irqrestore(&rtlpriv->locks.h2c_lock,
  76                                                        flag);
  77                                 h2c_waitcounter++;
  78                                 RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
  79                                          "Wait 100 us (%d times)...\n",
  80                                          h2c_waitcounter);
  81                                 udelay(100);
  82
  83                                 if (h2c_waitcounter > 1000)
  84                                         return;
  85                                 spin_lock_irqsave(&rtlpriv->locks.h2c_lock,
  86                                                   flag);
  87                         }
  88                         spin_unlock_irqrestore(&rtlpriv->locks.h2c_lock, flag);
  89                 } else {
  90                         rtlhal->h2c_setinprogress = true;
  91                         spin_unlock_irqrestore(&rtlpriv->locks.h2c_lock, flag);
  92                         break;
  93                 }
  94         }
  95         while (!bwrite_sucess) {
  96                 wait_writeh2c_limit--;
  97                 if (wait_writeh2c_limit == 0) {
  98                         RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
  99                                  "Write H2C fail because no trigger "
 100                                  "for FW INT!\n");
 101                         break;
 102                 }
 103                 boxnum = rtlhal->last_hmeboxnum;
 104                 switch (boxnum) {
 105                 case 0:
 106                         box_reg = REG_HMEBOX_0;
 107                         box_extreg = REG_HMEBOX_EXT_0;
 108                         break;
 109                 case 1:
 110                         box_reg = REG_HMEBOX_1;
 111                         box_extreg = REG_HMEBOX_EXT_1;
 112                         break;
 113                 case 2:
 114                         box_reg = REG_HMEBOX_2;
 115                         box_extreg = REG_HMEBOX_EXT_2;
 116                         break;
 117                 case 3:
 118                         box_reg = REG_HMEBOX_3;
 119                         box_extreg = REG_HMEBOX_EXT_3;
 120                         break;
 121                 default:
 122                         RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
 123                                  "switch case not processed\n");
 124                         break;
 125                 }
 126                 isfw_read = _rtl8723be_check_fw_read_last_h2c(hw, boxnum);
 127                 while (!isfw_read) {
 128                         wait_h2c_limit--;
 129                         if (wait_h2c_limit == 0) {
 130                                 RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
 131                                          "Wating too long for FW read "
 132                                          "clear HMEBox(%d)!\n", boxnum);
 133                                 break;
 134                         }
 135                         udelay(10);
 136
 137                         isfw_read = _rtl8723be_check_fw_read_last_h2c(hw,
 138                                                                 boxnum);
 139                         u1b_tmp = rtl_read_byte(rtlpriv, 0x130);
 140                         RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
 141                                  "Wating for FW read clear HMEBox(%d)!!! 0x130 = %2x\n",
 142                                  boxnum, u1b_tmp);
 143                 }
 144                 if (!isfw_read) {
 145                         RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
 146                                  "Write H2C register BOX[%d] fail!!!!! "
 147                                  "Fw do not read.\n", boxnum);
 148                         break;
 149                 }
 150                 memset(boxcontent, 0, sizeof(boxcontent));
 151                 memset(boxextcontent, 0, sizeof(boxextcontent));
 152                 boxcontent[0] = element_id;
 153                 RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
 154                          "Write element_id box_reg(%4x) = %2x\n",
 155                          box_reg, element_id);
 156
 157                 switch (cmd_len) {
 158                 case 1:
 159                 case 2:
 160                 case 3:
 161                         /*boxcontent[0] &= ~(BIT(7));*/
 162                         memcpy((u8 *)(boxcontent) + 1,
 163                                p_cmdbuffer + buf_index, cmd_len);
 164
 165                         for (idx = 0; idx < 4; idx++) {
 166                                 rtl_write_byte(rtlpriv, box_reg + idx,
 167                                                boxcontent[idx]);
 168                         }
 169                         break;
 170                 case 4:
 171                 case 5:
 172                 case 6:
 173                 case 7:
 174                         /*boxcontent[0] |= (BIT(7));*/
 175                         memcpy((u8 *)(boxextcontent),
 176                                p_cmdbuffer + buf_index+3, cmd_len-3);
 177                         memcpy((u8 *)(boxcontent) + 1,
 178                                p_cmdbuffer + buf_index, 3);
 179
 180                         for (idx = 0; idx < 4; idx++) {
 181                                 rtl_write_byte(rtlpriv, box_extreg + idx,
 182                                                boxextcontent[idx]);
 183                         }
 184                         for (idx = 0; idx < 4; idx++) {
 185                                 rtl_write_byte(rtlpriv, box_reg + idx,
 186                                                boxcontent[idx]);
 187                         }
 188                         break;
 189                 default:
 190                         RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
 191                                  "switch case not process\n");
 192                         break;
 193                 }
 194                 bwrite_sucess = true;
 195
 196                 rtlhal->last_hmeboxnum = boxnum + 1;
 197                 if (rtlhal->last_hmeboxnum == 4)
 198                         rtlhal->last_hmeboxnum = 0;
 199
 200                 RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
 201                          "pHalData->last_hmeboxnum  = %d\n",
 202                          rtlhal->last_hmeboxnum);
 203         }
 204         spin_lock_irqsave(&rtlpriv->locks.h2c_lock, flag);
 205         rtlhal->h2c_setinprogress = false;
 206         spin_unlock_irqrestore(&rtlpriv->locks.h2c_lock, flag);
 207
 208         RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD, "go out\n");
 209 }
 210
 211 void rtl8723be_fill_h2c_cmd(struct ieee80211_hw *hw, u8 element_id,
 212                             u32 cmd_len, u8 *p_cmdbuffer)
 213 {
 214         struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
 215         u32 tmp_cmdbuf[2];
 216
 217         if (!rtlhal->fw_ready) {
 218                 RT_ASSERT(false,
 219                           "return H2C cmd because of Fw download fail!!!\n");
 220                 return;
 221         }
 222         memset(tmp_cmdbuf, 0, 8);
 223         memcpy(tmp_cmdbuf, p_cmdbuffer, cmd_len);
 224         _rtl8723be_fill_h2c_command(hw, element_id, cmd_len,
 225                                     (u8 *)&tmp_cmdbuf);
 226         return;
 227 }
 228
 229 void rtl8723be_set_fw_pwrmode_cmd(struct ieee80211_hw *hw, u8 mode)
 230 {
 231         struct rtl_priv *rtlpriv = rtl_priv(hw);
 232         u8 u1_h2c_set_pwrmode[H2C_8723BE_PWEMODE_LENGTH] = { 0 };
 233         struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
 234         u8 rlbm, power_state = 0;
 235         RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD, "FW LPS mode = %d\n", mode);
 236
 237         SET_H2CCMD_PWRMODE_PARM_MODE(u1_h2c_set_pwrmode, ((mode) ? 1 : 0));
 238         rlbm = 0;/*YJ, temp, 120316. FW now not support RLBM = 2.*/
 239         SET_H2CCMD_PWRMODE_PARM_RLBM(u1_h2c_set_pwrmode, rlbm);
 240         SET_H2CCMD_PWRMODE_PARM_SMART_PS(u1_h2c_set_pwrmode,
 241                                          (rtlpriv->mac80211.p2p) ?
 242                                          ppsc->smart_ps : 1);
 243         SET_H2CCMD_PWRMODE_PARM_AWAKE_INTERVAL(u1_h2c_set_pwrmode,
 244                                                ppsc->reg_max_lps_awakeintvl);
 245         SET_H2CCMD_PWRMODE_PARM_ALL_QUEUE_UAPSD(u1_h2c_set_pwrmode, 0);
 246         if (mode == FW_PS_ACTIVE_MODE)
 247                 power_state |= FW_PWR_STATE_ACTIVE;
 248         else
 249                 power_state |= FW_PWR_STATE_RF_OFF;
 250         SET_H2CCMD_PWRMODE_PARM_PWR_STATE(u1_h2c_set_pwrmode, power_state);
 251
 252         RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_DMESG,
 253                       "rtl92c_set_fw_pwrmode(): u1_h2c_set_pwrmode\n",
 254                       u1_h2c_set_pwrmode, H2C_8723BE_PWEMODE_LENGTH);
 255         rtl8723be_fill_h2c_cmd(hw, H2C_8723BE_SETPWRMODE,
 256                                H2C_8723BE_PWEMODE_LENGTH,
 257                                u1_h2c_set_pwrmode);
 258 }
 259
 260 static bool _rtl8723be_cmd_send_packet(struct ieee80211_hw *hw,
 261                                        struct sk_buff *skb)
 262 {
 263         struct rtl_priv *rtlpriv = rtl_priv(hw);
 264         struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
 265         struct rtl8192_tx_ring *ring;
 266         struct rtl_tx_desc *pdesc;
 267         struct sk_buff *pskb = NULL;
 268         u8 own;
 269         unsigned long flags;
 270
 271         ring = &rtlpci->tx_ring[BEACON_QUEUE];
 272
 273         pskb = __skb_dequeue(&ring->queue);
 274         if (pskb)
 275                 kfree_skb(pskb);
 276
 277         spin_lock_irqsave(&rtlpriv->locks.irq_th_lock, flags);
 278
 279         pdesc = &ring->desc[0];
 280         own = (u8) rtlpriv->cfg->ops->get_desc((u8 *)pdesc, true, HW_DESC_OWN);
 281
 282         rtlpriv->cfg->ops->fill_tx_cmddesc(hw, (u8 *)pdesc, 1, 1, skb);
 283
 284         __skb_queue_tail(&ring->queue, skb);
 285
 286         spin_unlock_irqrestore(&rtlpriv->locks.irq_th_lock, flags);
 287
 288         rtlpriv->cfg->ops->tx_polling(hw, BEACON_QUEUE);
 289
 290         return true;
 291 }
 292 #define BEACON_PG               0 /* ->1 */
 293 #define PSPOLL_PG               2
 294 #define NULL_PG                 3
 295 #define PROBERSP_PG             4 /* ->5 */
 296
 297 #define TOTAL_RESERVED_PKT_LEN  768
 298
 299 static u8 reserved_page_packet[TOTAL_RESERVED_PKT_LEN] = {
 300         /* page 0 beacon */
 301         0x80, 0x00, 0x00, 0x00, 0xFF, 0xFF, 0xFF, 0xFF,
 302         0xFF, 0xFF, 0x00, 0xE0, 0x4C, 0x02, 0xB1, 0x78,
 303         0xEC, 0x1A, 0x59, 0x0B, 0xAD, 0xD4, 0x20, 0x00,
 304         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 305         0x64, 0x00, 0x10, 0x04, 0x00, 0x05, 0x54, 0x65,
 306         0x73, 0x74, 0x32, 0x01, 0x08, 0x82, 0x84, 0x0B,
 307         0x16, 0x24, 0x30, 0x48, 0x6C, 0x03, 0x01, 0x06,
 308         0x06, 0x02, 0x00, 0x00, 0x2A, 0x01, 0x02, 0x32,
 309         0x04, 0x0C, 0x12, 0x18, 0x60, 0x2D, 0x1A, 0x6C,
 310         0x09, 0x03, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
 311         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 312         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 313         0x00, 0x3D, 0x00, 0xDD, 0x07, 0x00, 0xE0, 0x4C,
 314         0x02, 0x02, 0x00, 0x00, 0xDD, 0x18, 0x00, 0x50,
 315         0xF2, 0x01, 0x01, 0x00, 0x00, 0x50, 0xF2, 0x04,
 316         0x01, 0x00, 0x00, 0x50, 0xF2, 0x04, 0x01, 0x00,
 317
 318         /* page 1 beacon */
 319         0x00, 0x50, 0xF2, 0x02, 0x00, 0x00, 0x00, 0x00,
 320         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 321         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 322         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 323         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 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         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 328         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 329         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 330         0x10, 0x00, 0x28, 0x8C, 0x00, 0x12, 0x00, 0x00,
 331         0x00, 0x00, 0x00, 0x00, 0x00, 0x81, 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
 336         /* page 2  ps-poll */
 337         0xA4, 0x10, 0x01, 0xC0, 0xEC, 0x1A, 0x59, 0x0B,
 338         0xAD, 0xD4, 0x00, 0xE0, 0x4C, 0x02, 0xB1, 0x78,
 339         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 340         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 341         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 342         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 343         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 344         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 345         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 346         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 347         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 348         0x18, 0x00, 0x28, 0x8C, 0x00, 0x12, 0x00, 0x00,
 349         0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00,
 350         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 351         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 352         0x00, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 353
 354         /* page 3  null */
 355         0x48, 0x01, 0x00, 0x00, 0xEC, 0x1A, 0x59, 0x0B,
 356         0xAD, 0xD4, 0x00, 0xE0, 0x4C, 0x02, 0xB1, 0x78,
 357         0xEC, 0x1A, 0x59, 0x0B, 0xAD, 0xD4, 0x00, 0x00,
 358         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 359         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 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         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 364         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 365         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 366         0x72, 0x00, 0x28, 0x8C, 0x00, 0x12, 0x00, 0x00,
 367         0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00,
 368         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 369         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 370         0x00, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 371
 372         /* page 4  probe_resp */
 373         0x50, 0x00, 0x00, 0x00, 0x00, 0x40, 0x10, 0x10,
 374         0x00, 0x03, 0x00, 0xE0, 0x4C, 0x76, 0x00, 0x42,
 375         0x00, 0x40, 0x10, 0x10, 0x00, 0x03, 0x00, 0x00,
 376         0x9E, 0x46, 0x15, 0x32, 0x27, 0xF2, 0x2D, 0x00,
 377         0x64, 0x00, 0x00, 0x04, 0x00, 0x0C, 0x6C, 0x69,
 378         0x6E, 0x6B, 0x73, 0x79, 0x73, 0x5F, 0x77, 0x6C,
 379         0x61, 0x6E, 0x01, 0x04, 0x82, 0x84, 0x8B, 0x96,
 380         0x03, 0x01, 0x01, 0x06, 0x02, 0x00, 0x00, 0x2A,
 381         0x01, 0x00, 0x32, 0x08, 0x24, 0x30, 0x48, 0x6C,
 382         0x0C, 0x12, 0x18, 0x60, 0x2D, 0x1A, 0x6C, 0x18,
 383         0x03, 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         0x3D, 0x00, 0xDD, 0x06, 0x00, 0xE0, 0x4C, 0x02,
 387         0x01, 0x70, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 388         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 389
 390         /* page 5  probe_resp */
 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         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 400         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 401         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 402         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 403         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 404         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 405         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 406         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 407 };
 408
 409 void rtl8723be_set_fw_rsvdpagepkt(struct ieee80211_hw *hw,
 410                                   bool dl_finished)
 411 {
 412         struct rtl_priv *rtlpriv = rtl_priv(hw);
 413         struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
 414         struct sk_buff *skb = NULL;
 415
 416         u32 totalpacketlen;
 417         bool rtstatus;
 418         u8 u1rsvdpageloc[5] = { 0 };
 419         bool dlok = false;
 420
 421         u8 *beacon;
 422         u8 *p_pspoll;
 423         u8 *nullfunc;
 424         u8 *p_probersp;
 425         /*---------------------------------------------------------
 426          *                      (1) beacon
 427          *---------------------------------------------------------
 428          */
 429         beacon = &reserved_page_packet[BEACON_PG * 128];
 430         SET_80211_HDR_ADDRESS2(beacon, mac->mac_addr);
 431         SET_80211_HDR_ADDRESS3(beacon, mac->bssid);
 432
 433         /*-------------------------------------------------------
 434          *                      (2) ps-poll
 435          *-------------------------------------------------------
 436          */
 437         p_pspoll = &reserved_page_packet[PSPOLL_PG * 128];
 438         SET_80211_PS_POLL_AID(p_pspoll, (mac->assoc_id | 0xc000));
 439         SET_80211_PS_POLL_BSSID(p_pspoll, mac->bssid);
 440         SET_80211_PS_POLL_TA(p_pspoll, mac->mac_addr);
 441
 442         SET_H2CCMD_RSVDPAGE_LOC_PSPOLL(u1rsvdpageloc, PSPOLL_PG);
 443
 444         /*--------------------------------------------------------
 445          *                      (3) null data
 446          *--------------------------------------------------------
 447          */
 448         nullfunc = &reserved_page_packet[NULL_PG * 128];
 449         SET_80211_HDR_ADDRESS1(nullfunc, mac->bssid);
 450         SET_80211_HDR_ADDRESS2(nullfunc, mac->mac_addr);
 451         SET_80211_HDR_ADDRESS3(nullfunc, mac->bssid);
 452
 453         SET_H2CCMD_RSVDPAGE_LOC_NULL_DATA(u1rsvdpageloc, NULL_PG);
 454
 455         /*---------------------------------------------------------
 456          *                      (4) probe response
 457          *---------------------------------------------------------
 458          */
 459         p_probersp = &reserved_page_packet[PROBERSP_PG * 128];
 460         SET_80211_HDR_ADDRESS1(p_probersp, mac->bssid);
 461         SET_80211_HDR_ADDRESS2(p_probersp, mac->mac_addr);
 462         SET_80211_HDR_ADDRESS3(p_probersp, mac->bssid);
 463
 464         SET_H2CCMD_RSVDPAGE_LOC_PROBE_RSP(u1rsvdpageloc, PROBERSP_PG);
 465
 466         totalpacketlen = TOTAL_RESERVED_PKT_LEN;
 467
 468         RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_LOUD,
 469                       "rtl8723be_set_fw_rsvdpagepkt(): "
 470                       "HW_VAR_SET_TX_CMD: ALL\n",
 471                       &reserved_page_packet[0], totalpacketlen);
 472         RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_DMESG,
 473                       "rtl8723be_set_fw_rsvdpagepkt(): "
 474                       "HW_VAR_SET_TX_CMD: ALL\n", u1rsvdpageloc, 3);
 475
 476
 477         skb = dev_alloc_skb(totalpacketlen);
 478         memcpy((u8 *)skb_put(skb, totalpacketlen),
 479                &reserved_page_packet, totalpacketlen);
 480
 481         rtstatus = _rtl8723be_cmd_send_packet(hw, skb);
 482
 483         if (rtstatus)
 484                 dlok = true;
 485
 486         if (dlok) {
 487                 RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD,
 488                          "Set RSVD page location to Fw.\n");
 489                 RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_DMESG, "H2C_RSVDPAGE:\n",
 490                               u1rsvdpageloc, 3);
 491                 rtl8723be_fill_h2c_cmd(hw, H2C_8723BE_RSVDPAGE,
 492                                        sizeof(u1rsvdpageloc), u1rsvdpageloc);
 493         } else {
 494                 RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
 495                          "Set RSVD page location to Fw FAIL!!!!!!.\n");
 496         }
 497 }
 498
 499 /*Should check FW support p2p or not.*/
 500 static void rtl8723be_set_p2p_ctw_period_cmd(struct ieee80211_hw *hw,
 501                                              u8 ctwindow)
 502 {
 503         u8 u1_ctwindow_period[1] = {ctwindow};
 504
 505         rtl8723be_fill_h2c_cmd(hw, H2C_8723BE_P2P_PS_CTW_CMD, 1,
 506                                u1_ctwindow_period);
 507 }
 508
 509 void rtl8723be_set_p2p_ps_offload_cmd(struct ieee80211_hw *hw,
 510                                       u8 p2p_ps_state)
 511 {
 512         struct rtl_priv *rtlpriv = rtl_priv(hw);
 513         struct rtl_ps_ctl *rtlps = rtl_psc(rtl_priv(hw));
 514         struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
 515         struct rtl_p2p_ps_info *p2pinfo = &(rtlps->p2p_ps_info);
 516         struct p2p_ps_offload_t *p2p_ps_offload = &rtlhal->p2p_ps_offload;
 517         u8 i;
 518         u16 ctwindow;
 519         u32 start_time, tsf_low;
 520
 521         switch (p2p_ps_state) {
 522         case P2P_PS_DISABLE:
 523                 RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "P2P_PS_DISABLE\n");
 524                 memset(p2p_ps_offload, 0, sizeof(struct p2p_ps_offload_t));
 525                 break;
 526         case P2P_PS_ENABLE:
 527                 RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "P2P_PS_ENABLE\n");
 528                 /* update CTWindow value. */
 529                 if (p2pinfo->ctwindow > 0) {
 530                         p2p_ps_offload->ctwindow_en = 1;
 531                         ctwindow = p2pinfo->ctwindow;
 532                         rtl8723be_set_p2p_ctw_period_cmd(hw, ctwindow);
 533                 }
 534                 /* hw only support 2 set of NoA */
 535                 for (i = 0; i < p2pinfo->noa_num; i++) {
 536                         /* To control the register setting
 537                          * for which NOA
 538                          */
 539                         rtl_write_byte(rtlpriv, 0x5cf, (i << 4));
 540                         if (i == 0)
 541                                 p2p_ps_offload->noa0_en = 1;
 542                         else
 543                                 p2p_ps_offload->noa1_en = 1;
 544
 545                         /* config P2P NoA Descriptor Register */
 546                         rtl_write_dword(rtlpriv, 0x5E0,
 547                                         p2pinfo->noa_duration[i]);
 548                         rtl_write_dword(rtlpriv, 0x5E4,
 549                                         p2pinfo->noa_interval[i]);
 550
 551                         /*Get Current TSF value */
 552                         tsf_low = rtl_read_dword(rtlpriv, REG_TSFTR);
 553
 554                         start_time = p2pinfo->noa_start_time[i];
 555                         if (p2pinfo->noa_count_type[i] != 1) {
 556                                 while (start_time <= (tsf_low + (50 * 1024))) {
 557                                         start_time += p2pinfo->noa_interval[i];
 558                                         if (p2pinfo->noa_count_type[i] != 255)
 559                                                 p2pinfo->noa_count_type[i]--;
 560                                 }
 561                         }
 562                         rtl_write_dword(rtlpriv, 0x5E8, start_time);
 563                         rtl_write_dword(rtlpriv, 0x5EC,
 564                                         p2pinfo->noa_count_type[i]);
 565                 }
 566                 if ((p2pinfo->opp_ps == 1) ||
 567                     (p2pinfo->noa_num > 0)) {
 568                         /* rst p2p circuit */
 569                         rtl_write_byte(rtlpriv, REG_DUAL_TSF_RST, BIT(4));
 570
 571                         p2p_ps_offload->offload_en = 1;
 572
 573                         if (P2P_ROLE_GO == rtlpriv->mac80211.p2p) {
 574                                 p2p_ps_offload->role = 1;
 575                                 p2p_ps_offload->allstasleep = 0;
 576                         } else {
 577                                 p2p_ps_offload->role = 0;
 578                         }
 579                         p2p_ps_offload->discovery = 0;
 580                 }
 581                 break;
 582         case P2P_PS_SCAN:
 583                 RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "P2P_PS_SCAN\n");
 584                 p2p_ps_offload->discovery = 1;
 585                 break;
 586         case P2P_PS_SCAN_DONE:
 587                 RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "P2P_PS_SCAN_DONE\n");
 588                 p2p_ps_offload->discovery = 0;
 589                 p2pinfo->p2p_ps_state = P2P_PS_ENABLE;
 590                 break;
 591         default:
 592                 break;
 593         }
 594         rtl8723be_fill_h2c_cmd(hw, H2C_8723BE_P2P_PS_OFFLOAD, 1,
 595                                (u8 *)p2p_ps_offload);
 596 }
 597
 598 void rtl8723be_set_fw_joinbss_report_cmd(struct ieee80211_hw *hw, u8 mstatus)
 599 {
 600         u8 u1_joinbssrpt_parm[1] = { 0 };
 601
 602         SET_H2CCMD_JOINBSSRPT_PARM_OPMODE(u1_joinbssrpt_parm, mstatus);
 603
 604         rtl8723be_fill_h2c_cmd(hw, H2C_8723BE_JOINBSSRPT, 1,
 605                                u1_joinbssrpt_parm);
 606 }
 607
 608 void rtl8723be_set_fw_ap_off_load_cmd(struct ieee80211_hw *hw,
 609                                       u8 ap_offload_enable)
 610 {
 611         struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
 612         u8 u1_apoffload_parm[H2C_8723BE_AP_OFFLOAD_LENGTH] = { 0 };
 613
 614         SET_H2CCMD_AP_OFFLOAD_ON(u1_apoffload_parm, ap_offload_enable);
 615         SET_H2CCMD_AP_OFFLOAD_HIDDEN(u1_apoffload_parm, mac->hiddenssid);
 616         SET_H2CCMD_AP_OFFLOAD_DENYANY(u1_apoffload_parm, 0);
 617
 618         rtl8723be_fill_h2c_cmd(hw, H2C_8723BE_AP_OFFLOAD,
 619                                H2C_8723BE_AP_OFFLOAD_LENGTH, u1_apoffload_parm);
 620 }