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

   1 /******************************************************************************
   2  *
   3  * Copyright(c) 2009-2012  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 _rtl8723e_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, val_mcutst_1;
  40         bool result = false;
  41
  42         val_hmetfr = rtl_read_byte(rtlpriv, REG_HMETFR);
  43         val_mcutst_1 = rtl_read_byte(rtlpriv, (REG_MCUTST_1 + boxnum));
  44
  45         if (((val_hmetfr >> boxnum) & BIT(0)) == 0 && val_mcutst_1 == 0)
  46                 result = true;
  47         return result;
  48 }
  49
  50 static void _rtl8723e_fill_h2c_command(struct ieee80211_hw *hw, u8 element_id,
  51                                        u32 cmd_len, u8 *cmdbuffer)
  52 {
  53         struct rtl_priv *rtlpriv = rtl_priv(hw);
  54         struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
  55         u8 boxnum;
  56         u16 box_reg = 0, box_extreg = 0;
  57         u8 u1b_tmp;
  58         bool isfw_read = false;
  59         u8 buf_index = 0;
  60         bool bwrite_sucess = false;
  61         u8 wait_h2c_limmit = 100;
  62         u8 wait_writeh2c_limmit = 100;
  63         u8 boxcontent[4], boxextcontent[2];
  64         u32 h2c_waitcounter = 0;
  65         unsigned long flag;
  66         u8 idx;
  67
  68         RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD, "come in\n");
  69
  70         while (true) {
  71                 spin_lock_irqsave(&rtlpriv->locks.h2c_lock, flag);
  72                 if (rtlhal->h2c_setinprogress) {
  73                         RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
  74                                  "H2C set in progress! Wait to set..element_id(%d).\n",
  75                                  element_id);
  76
  77                         while (rtlhal->h2c_setinprogress) {
  78                                 spin_unlock_irqrestore(&rtlpriv->locks.h2c_lock,
  79                                                        flag);
  80                                 h2c_waitcounter++;
  81                                 RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
  82                                          "Wait 100 us (%d times)...\n",
  83                                           h2c_waitcounter);
  84                                 udelay(100);
  85
  86                                 if (h2c_waitcounter > 1000)
  87                                         return;
  88                                 spin_lock_irqsave(&rtlpriv->locks.h2c_lock,
  89                                                   flag);
  90                         }
  91                         spin_unlock_irqrestore(&rtlpriv->locks.h2c_lock, flag);
  92                 } else {
  93                         rtlhal->h2c_setinprogress = true;
  94                         spin_unlock_irqrestore(&rtlpriv->locks.h2c_lock, flag);
  95                         break;
  96                 }
  97         }
  98
  99         while (!bwrite_sucess) {
 100                 wait_writeh2c_limmit--;
 101                 if (wait_writeh2c_limmit == 0) {
 102                         RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
 103                                  "Write H2C fail because no trigger for FW INT!\n");
 104                         break;
 105                 }
 106
 107                 boxnum = rtlhal->last_hmeboxnum;
 108                 switch (boxnum) {
 109                 case 0:
 110                         box_reg = REG_HMEBOX_0;
 111                         box_extreg = REG_HMEBOX_EXT_0;
 112                         break;
 113                 case 1:
 114                         box_reg = REG_HMEBOX_1;
 115                         box_extreg = REG_HMEBOX_EXT_1;
 116                         break;
 117                 case 2:
 118                         box_reg = REG_HMEBOX_2;
 119                         box_extreg = REG_HMEBOX_EXT_2;
 120                         break;
 121                 case 3:
 122                         box_reg = REG_HMEBOX_3;
 123                         box_extreg = REG_HMEBOX_EXT_3;
 124                         break;
 125                 default:
 126                         RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
 127                                  "switch case not process\n");
 128                         break;
 129                 }
 130
 131                 isfw_read = _rtl8723e_check_fw_read_last_h2c(hw, boxnum);
 132                 while (!isfw_read) {
 133
 134                         wait_h2c_limmit--;
 135                         if (wait_h2c_limmit == 0) {
 136                                 RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
 137                                          "Wating too long for FW read clear HMEBox(%d)!\n",
 138                                          boxnum);
 139                                 break;
 140                         }
 141
 142                         udelay(10);
 143
 144                         isfw_read = _rtl8723e_check_fw_read_last_h2c(hw,
 145                                                                 boxnum);
 146                         u1b_tmp = rtl_read_byte(rtlpriv, 0x1BF);
 147                         RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
 148                                  "Waiting for FW read clear HMEBox(%d)!!! 0x1BF = %2x\n",
 149                                  boxnum, u1b_tmp);
 150                 }
 151
 152                 if (!isfw_read) {
 153                         RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
 154                                  "Write H2C register BOX[%d] fail!!!!! Fw do not read.\n",
 155                                  boxnum);
 156                         break;
 157                 }
 158
 159                 memset(boxcontent, 0, sizeof(boxcontent));
 160                 memset(boxextcontent, 0, sizeof(boxextcontent));
 161                 boxcontent[0] = element_id;
 162                 RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
 163                          "Write element_id box_reg(%4x) = %2x\n",
 164                           box_reg, element_id);
 165
 166                 switch (cmd_len) {
 167                 case 1:
 168                         boxcontent[0] &= ~(BIT(7));
 169                         memcpy((u8 *)(boxcontent) + 1,
 170                                cmdbuffer + buf_index, 1);
 171
 172                         for (idx = 0; idx < 4; idx++) {
 173                                 rtl_write_byte(rtlpriv, box_reg + idx,
 174                                                boxcontent[idx]);
 175                         }
 176                         break;
 177                 case 2:
 178                         boxcontent[0] &= ~(BIT(7));
 179                         memcpy((u8 *)(boxcontent) + 1,
 180                                cmdbuffer + buf_index, 2);
 181
 182                         for (idx = 0; idx < 4; idx++) {
 183                                 rtl_write_byte(rtlpriv, box_reg + idx,
 184                                                boxcontent[idx]);
 185                         }
 186                         break;
 187                 case 3:
 188                         boxcontent[0] &= ~(BIT(7));
 189                         memcpy((u8 *)(boxcontent) + 1,
 190                                cmdbuffer + buf_index, 3);
 191
 192                         for (idx = 0; idx < 4; idx++) {
 193                                 rtl_write_byte(rtlpriv, box_reg + idx,
 194                                                boxcontent[idx]);
 195                         }
 196                         break;
 197                 case 4:
 198                         boxcontent[0] |= (BIT(7));
 199                         memcpy((u8 *)(boxextcontent),
 200                                cmdbuffer + buf_index, 2);
 201                         memcpy((u8 *)(boxcontent) + 1,
 202                                cmdbuffer + buf_index + 2, 2);
 203
 204                         for (idx = 0; idx < 2; idx++) {
 205                                 rtl_write_byte(rtlpriv, box_extreg + idx,
 206                                                boxextcontent[idx]);
 207                         }
 208
 209                         for (idx = 0; idx < 4; idx++) {
 210                                 rtl_write_byte(rtlpriv, box_reg + idx,
 211                                                boxcontent[idx]);
 212                         }
 213                         break;
 214                 case 5:
 215                         boxcontent[0] |= (BIT(7));
 216                         memcpy((u8 *)(boxextcontent),
 217                                cmdbuffer + buf_index, 2);
 218                         memcpy((u8 *)(boxcontent) + 1,
 219                                cmdbuffer + buf_index + 2, 3);
 220
 221                         for (idx = 0; idx < 2; idx++) {
 222                                 rtl_write_byte(rtlpriv, box_extreg + idx,
 223                                                boxextcontent[idx]);
 224                         }
 225
 226                         for (idx = 0; idx < 4; idx++) {
 227                                 rtl_write_byte(rtlpriv, box_reg + idx,
 228                                                boxcontent[idx]);
 229                         }
 230                         break;
 231                 default:
 232                         RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
 233                                  "switch case not process\n");
 234                         break;
 235                 }
 236
 237                 bwrite_sucess = true;
 238
 239                 rtlhal->last_hmeboxnum = boxnum + 1;
 240                 if (rtlhal->last_hmeboxnum == 4)
 241                         rtlhal->last_hmeboxnum = 0;
 242
 243                 RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
 244                          "pHalData->last_hmeboxnum  = %d\n",
 245                           rtlhal->last_hmeboxnum);
 246         }
 247
 248         spin_lock_irqsave(&rtlpriv->locks.h2c_lock, flag);
 249         rtlhal->h2c_setinprogress = false;
 250         spin_unlock_irqrestore(&rtlpriv->locks.h2c_lock, flag);
 251
 252         RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD, "go out\n");
 253 }
 254
 255 void rtl8723e_fill_h2c_cmd(struct ieee80211_hw *hw,
 256                            u8 element_id, u32 cmd_len, u8 *cmdbuffer)
 257 {
 258         struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
 259         u32 tmp_cmdbuf[2];
 260
 261         if (!rtlhal->fw_ready) {
 262                 RT_ASSERT(false,
 263                           "return H2C cmd because of Fw download fail!!!\n");
 264                 return;
 265         }
 266         memset(tmp_cmdbuf, 0, 8);
 267         memcpy(tmp_cmdbuf, cmdbuffer, cmd_len);
 268         _rtl8723e_fill_h2c_command(hw, element_id, cmd_len,
 269                                    (u8 *)&tmp_cmdbuf);
 270 }
 271
 272 void rtl8723e_set_fw_pwrmode_cmd(struct ieee80211_hw *hw, u8 mode)
 273 {
 274         struct rtl_priv *rtlpriv = rtl_priv(hw);
 275         u8 u1_h2c_set_pwrmode[3] = { 0 };
 276         struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
 277
 278         RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD, "FW LPS mode = %d\n", mode);
 279
 280         SET_H2CCMD_PWRMODE_PARM_MODE(u1_h2c_set_pwrmode, mode);
 281         SET_H2CCMD_PWRMODE_PARM_SMART_PS(u1_h2c_set_pwrmode,
 282                 (rtlpriv->mac80211.p2p) ? ppsc->smart_ps : 1);
 283         SET_H2CCMD_PWRMODE_PARM_BCN_PASS_TIME(u1_h2c_set_pwrmode,
 284                                               ppsc->reg_max_lps_awakeintvl);
 285
 286         RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_DMESG,
 287                       "rtl8723e_set_fw_rsvdpagepkt(): u1_h2c_set_pwrmode\n",
 288                       u1_h2c_set_pwrmode, 3);
 289         rtl8723e_fill_h2c_cmd(hw, H2C_SETPWRMODE, 3, u1_h2c_set_pwrmode);
 290 }
 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, 0x76, 0x00, 0x42,
 303         0x00, 0x40, 0x10, 0x10, 0x00, 0x03, 0x50, 0x08,
 304         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 305         0x64, 0x00, 0x00, 0x04, 0x00, 0x0C, 0x6C, 0x69,
 306         0x6E, 0x6B, 0x73, 0x79, 0x73, 0x5F, 0x77, 0x6C,
 307         0x61, 0x6E, 0x01, 0x04, 0x82, 0x84, 0x8B, 0x96,
 308         0x03, 0x01, 0x01, 0x06, 0x02, 0x00, 0x00, 0x2A,
 309         0x01, 0x00, 0x32, 0x08, 0x24, 0x30, 0x48, 0x6C,
 310         0x0C, 0x12, 0x18, 0x60, 0x2D, 0x1A, 0x6C, 0x18,
 311         0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 312         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 313         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 314         0x3D, 0x00, 0xDD, 0x06, 0x00, 0xE0, 0x4C, 0x02,
 315         0x01, 0x70, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 316         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 317
 318         /* page 1 beacon */
 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         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         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 331         0x10, 0x00, 0x20, 0x8C, 0x00, 0x12, 0x10, 0x00,
 332         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 333         0x00, 0x01, 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, 0x00, 0x40, 0x10, 0x10,
 338         0x00, 0x03, 0x00, 0xE0, 0x4C, 0x76, 0x00, 0x42,
 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         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 349         0x18, 0x00, 0x20, 0x8C, 0x00, 0x12, 0x00, 0x00,
 350         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x80,
 351         0x80, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 352         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 353
 354         /* page 3  null */
 355         0x48, 0x01, 0x00, 0x00, 0x00, 0x40, 0x10, 0x10,
 356         0x00, 0x03, 0x00, 0xE0, 0x4C, 0x76, 0x00, 0x42,
 357         0x00, 0x40, 0x10, 0x10, 0x00, 0x03, 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         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 367         0x72, 0x00, 0x20, 0x8C, 0x00, 0x12, 0x00, 0x00,
 368         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x80,
 369         0x80, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 370         0x00, 0x00, 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 rtl8723e_set_fw_rsvdpagepkt(struct ieee80211_hw *hw, bool b_dl_finished)
 410 {
 411         struct rtl_priv *rtlpriv = rtl_priv(hw);
 412         struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
 413         struct sk_buff *skb = NULL;
 414         u32 totalpacketlen;
 415         bool rtstatus;
 416         u8 u1rsvdpageloc[3] = { 0 };
 417         bool b_dlok = false;
 418         u8 *beacon;
 419         u8 *p_pspoll;
 420         u8 *nullfunc;
 421         u8 *p_probersp;
 422
 423         /*---------------------------------------------------------
 424          *                      (1) beacon
 425          *---------------------------------------------------------
 426          */
 427         beacon = &reserved_page_packet[BEACON_PG * 128];
 428         SET_80211_HDR_ADDRESS2(beacon, mac->mac_addr);
 429         SET_80211_HDR_ADDRESS3(beacon, mac->bssid);
 430
 431         /*-------------------------------------------------------
 432          *                      (2) ps-poll
 433          *--------------------------------------------------------
 434          */
 435         p_pspoll = &reserved_page_packet[PSPOLL_PG * 128];
 436         SET_80211_PS_POLL_AID(p_pspoll, (mac->assoc_id | 0xc000));
 437         SET_80211_PS_POLL_BSSID(p_pspoll, mac->bssid);
 438         SET_80211_PS_POLL_TA(p_pspoll, mac->mac_addr);
 439
 440         SET_H2CCMD_RSVDPAGE_LOC_PSPOLL(u1rsvdpageloc, PSPOLL_PG);
 441
 442         /*--------------------------------------------------------
 443          *                      (3) null data
 444          *---------------------------------------------------------
 445          */
 446         nullfunc = &reserved_page_packet[NULL_PG * 128];
 447         SET_80211_HDR_ADDRESS1(nullfunc, mac->bssid);
 448         SET_80211_HDR_ADDRESS2(nullfunc, mac->mac_addr);
 449         SET_80211_HDR_ADDRESS3(nullfunc, mac->bssid);
 450
 451         SET_H2CCMD_RSVDPAGE_LOC_NULL_DATA(u1rsvdpageloc, NULL_PG);
 452
 453         /*---------------------------------------------------------
 454          *                      (4) probe response
 455          *----------------------------------------------------------
 456          */
 457         p_probersp = &reserved_page_packet[PROBERSP_PG * 128];
 458         SET_80211_HDR_ADDRESS1(p_probersp, mac->bssid);
 459         SET_80211_HDR_ADDRESS2(p_probersp, mac->mac_addr);
 460         SET_80211_HDR_ADDRESS3(p_probersp, mac->bssid);
 461
 462         SET_H2CCMD_RSVDPAGE_LOC_PROBE_RSP(u1rsvdpageloc, PROBERSP_PG);
 463
 464         totalpacketlen = TOTAL_RESERVED_PKT_LEN;
 465
 466         RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_LOUD,
 467                       "rtl8723e_set_fw_rsvdpagepkt(): HW_VAR_SET_TX_CMD: ALL\n",
 468                       &reserved_page_packet[0], totalpacketlen);
 469         RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_DMESG,
 470                       "rtl8723e_set_fw_rsvdpagepkt(): HW_VAR_SET_TX_CMD: ALL\n",
 471                       u1rsvdpageloc, 3);
 472
 473         skb = dev_alloc_skb(totalpacketlen);
 474         memcpy((u8 *)skb_put(skb, totalpacketlen),
 475                &reserved_page_packet, totalpacketlen);
 476
 477         rtstatus = rtl_cmd_send_packet(hw, skb);
 478
 479         if (rtstatus)
 480                 b_dlok = true;
 481
 482         if (b_dlok) {
 483                 RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD,
 484                          "Set RSVD page location to Fw.\n");
 485                 RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_DMESG,
 486                               "H2C_RSVDPAGE:\n",
 487                               u1rsvdpageloc, 3);
 488                 rtl8723e_fill_h2c_cmd(hw, H2C_RSVDPAGE,
 489                                       sizeof(u1rsvdpageloc), u1rsvdpageloc);
 490         } else
 491                 RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
 492                          "Set RSVD page location to Fw FAIL!!!!!!.\n");
 493 }
 494
 495 void rtl8723e_set_fw_joinbss_report_cmd(struct ieee80211_hw *hw, u8 mstatus)
 496 {
 497         u8 u1_joinbssrpt_parm[1] = { 0 };
 498
 499         SET_H2CCMD_JOINBSSRPT_PARM_OPMODE(u1_joinbssrpt_parm, mstatus);
 500
 501         rtl8723e_fill_h2c_cmd(hw, H2C_JOINBSSRPT, 1, u1_joinbssrpt_parm);
 502 }
 503
 504 static void rtl8723e_set_p2p_ctw_period_cmd(struct ieee80211_hw *hw,
 505                                             u8 ctwindow)
 506 {
 507         u8 u1_ctwindow_period[1] = { ctwindow};
 508
 509         rtl8723e_fill_h2c_cmd(hw, H2C_P2P_PS_CTW_CMD, 1, u1_ctwindow_period);
 510
 511 }
 512
 513 void rtl8723e_set_p2p_ps_offload_cmd(struct ieee80211_hw *hw, u8 p2p_ps_state)
 514 {
 515         struct rtl_priv *rtlpriv = rtl_priv(hw);
 516         struct rtl_ps_ctl *rtlps = rtl_psc(rtl_priv(hw));
 517         struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
 518         struct rtl_p2p_ps_info *p2pinfo = &(rtlps->p2p_ps_info);
 519         struct p2p_ps_offload_t *p2p_ps_offload = &rtlhal->p2p_ps_offload;
 520         u8      i;
 521         u16     ctwindow;
 522         u32     start_time, tsf_low;
 523
 524         switch (p2p_ps_state) {
 525         case P2P_PS_DISABLE:
 526                 RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "P2P_PS_DISABLE\n");
 527                 memset(p2p_ps_offload, 0, sizeof(*p2p_ps_offload));
 528                 break;
 529         case P2P_PS_ENABLE:
 530                 RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "P2P_PS_ENABLE\n");
 531                 /* update CTWindow value. */
 532                 if (p2pinfo->ctwindow > 0) {
 533                         p2p_ps_offload->ctwindow_en = 1;
 534                         ctwindow = p2pinfo->ctwindow;
 535                         rtl8723e_set_p2p_ctw_period_cmd(hw, ctwindow);
 536                 }
 537
 538                 /* hw only support 2 set of NoA */
 539                 for (i = 0 ; i < p2pinfo->noa_num ; i++) {
 540                         /* To control the register setting for which NOA*/
 541                         rtl_write_byte(rtlpriv, 0x5cf, (i << 4));
 542                         if (i == 0)
 543                                 p2p_ps_offload->noa0_en = 1;
 544                         else
 545                                 p2p_ps_offload->noa1_en = 1;
 546
 547                         /* config P2P NoA Descriptor Register */
 548                         rtl_write_dword(rtlpriv, 0x5E0,
 549                                         p2pinfo->noa_duration[i]);
 550                         rtl_write_dword(rtlpriv, 0x5E4,
 551                                         p2pinfo->noa_interval[i]);
 552
 553                         /*Get Current TSF value */
 554                         tsf_low = rtl_read_dword(rtlpriv, REG_TSFTR);
 555
 556                         start_time = p2pinfo->noa_start_time[i];
 557                         if (p2pinfo->noa_count_type[i] != 1) {
 558                                 while (start_time <=
 559                                         (tsf_low+(50*1024))) {
 560                                         start_time +=
 561                                                 p2pinfo->noa_interval[i];
 562                                         if (p2pinfo->noa_count_type[i] != 255)
 563                                                 p2pinfo->noa_count_type[i]--;
 564                                 }
 565                         }
 566                         rtl_write_dword(rtlpriv, 0x5E8, start_time);
 567                         rtl_write_dword(rtlpriv, 0x5EC,
 568                                 p2pinfo->noa_count_type[i]);
 569
 570                 }
 571
 572                 if ((p2pinfo->opp_ps == 1) || (p2pinfo->noa_num > 0)) {
 573                         /* rst p2p circuit */
 574                         rtl_write_byte(rtlpriv, REG_DUAL_TSF_RST, BIT(4));
 575
 576                         p2p_ps_offload->offload_en = 1;
 577
 578                         if (P2P_ROLE_GO == rtlpriv->mac80211.p2p) {
 579                                 p2p_ps_offload->role = 1;
 580                                 p2p_ps_offload->allstasleep = 0;
 581                         } else {
 582                                 p2p_ps_offload->role = 0;
 583                         }
 584
 585                         p2p_ps_offload->discovery = 0;
 586                 }
 587                 break;
 588         case P2P_PS_SCAN:
 589                 RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "P2P_PS_SCAN\n");
 590                 p2p_ps_offload->discovery = 1;
 591                 break;
 592         case P2P_PS_SCAN_DONE:
 593                 RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "P2P_PS_SCAN_DONE\n");
 594                 p2p_ps_offload->discovery = 0;
 595                 p2pinfo->p2p_ps_state = P2P_PS_ENABLE;
 596                 break;
 597         default:
 598                 break;
 599         }
 600
 601         rtl8723e_fill_h2c_cmd(hw, H2C_P2P_PS_OFFLOAD, 1, (u8 *)p2p_ps_offload);
 602
 603 }