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

   1 /******************************************************************************
   2  *
   3  * Copyright(c) 2009-2013  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  * You should have received a copy of the GNU General Public License along with
  15  * this program; if not, write to the Free Software Foundation, Inc.,
  16  * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
  17  *
  18  * The full GNU General Public License is included in this distribution in the
  19  * file called LICENSE.
  20  *
  21  * Contact Information:
  22  * wlanfae <wlanfae@realtek.com>
  23  * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
  24  * Hsinchu 300, Taiwan.
  25  *
  26  * Larry Finger <Larry.Finger@lwfinger.net>
  27  *
  28  *****************************************************************************/
  29
  30 #include "../wifi.h"
  31 #include "../pci.h"
  32 #include "../base.h"
  33 #include "reg.h"
  34 #include "def.h"
  35 #include "fw.h"
  36
  37 #include <linux/kmemleak.h>
  38
  39 static void _rtl88e_enable_fw_download(struct ieee80211_hw *hw, bool enable)
  40 {
  41         struct rtl_priv *rtlpriv = rtl_priv(hw);
  42         u8 tmp;
  43
  44         if (enable) {
  45                 tmp = rtl_read_byte(rtlpriv, REG_SYS_FUNC_EN + 1);
  46                 rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN + 1, tmp | 0x04);
  47
  48                 tmp = rtl_read_byte(rtlpriv, REG_MCUFWDL);
  49                 rtl_write_byte(rtlpriv, REG_MCUFWDL, tmp | 0x01);
  50
  51                 tmp = rtl_read_byte(rtlpriv, REG_MCUFWDL + 2);
  52                 rtl_write_byte(rtlpriv, REG_MCUFWDL + 2, tmp & 0xf7);
  53         } else {
  54                 tmp = rtl_read_byte(rtlpriv, REG_MCUFWDL);
  55                 rtl_write_byte(rtlpriv, REG_MCUFWDL, tmp & 0xfe);
  56
  57                 rtl_write_byte(rtlpriv, REG_MCUFWDL + 1, 0x00);
  58         }
  59 }
  60
  61 static void _rtl88e_fw_block_write(struct ieee80211_hw *hw,
  62                                    const u8 *buffer, u32 size)
  63 {
  64         struct rtl_priv *rtlpriv = rtl_priv(hw);
  65         u32 blk_sz = sizeof(u32);
  66         u8 *buf_ptr = (u8 *)buffer;
  67         u32 *pu4BytePtr = (u32 *)buffer;
  68         u32 i, offset, blk_cnt, remain;
  69
  70         blk_cnt = size / blk_sz;
  71         remain = size % blk_sz;
  72
  73         for (i = 0; i < blk_cnt; i++) {
  74                 offset = i * blk_sz;
  75                 rtl_write_dword(rtlpriv, (FW_8192C_START_ADDRESS + offset),
  76                                 *(pu4BytePtr + i));
  77         }
  78
  79         if (remain) {
  80                 offset = blk_cnt * blk_sz;
  81                 buf_ptr += offset;
  82                 for (i = 0; i < remain; i++) {
  83                         rtl_write_byte(rtlpriv, (FW_8192C_START_ADDRESS +
  84                                                  offset + i), *(buf_ptr + i));
  85                 }
  86         }
  87 }
  88
  89 static void _rtl88e_fw_page_write(struct ieee80211_hw *hw,
  90                                   u32 page, const u8 *buffer, u32 size)
  91 {
  92         struct rtl_priv *rtlpriv = rtl_priv(hw);
  93         u8 value8;
  94         u8 u8page = (u8) (page & 0x07);
  95
  96         value8 = (rtl_read_byte(rtlpriv, REG_MCUFWDL + 2) & 0xF8) | u8page;
  97
  98         rtl_write_byte(rtlpriv, (REG_MCUFWDL + 2), value8);
  99         _rtl88e_fw_block_write(hw, buffer, size);
 100 }
 101
 102 static void _rtl88e_fill_dummy(u8 *pfwbuf, u32 *pfwlen)
 103 {
 104         u32 fwlen = *pfwlen;
 105         u8 remain = (u8) (fwlen % 4);
 106
 107         remain = (remain == 0) ? 0 : (4 - remain);
 108
 109         while (remain > 0) {
 110                 pfwbuf[fwlen] = 0;
 111                 fwlen++;
 112                 remain--;
 113         }
 114
 115         *pfwlen = fwlen;
 116 }
 117
 118 static void _rtl88e_write_fw(struct ieee80211_hw *hw,
 119                              enum version_8188e version, u8 *buffer, u32 size)
 120 {
 121         struct rtl_priv *rtlpriv = rtl_priv(hw);
 122         u8 *buf_ptr = (u8 *)buffer;
 123         u32 page_no, remain;
 124         u32 page, offset;
 125
 126         RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "FW size is %d bytes,\n", size);
 127
 128         _rtl88e_fill_dummy(buf_ptr, &size);
 129
 130         page_no = size / FW_8192C_PAGE_SIZE;
 131         remain = size % FW_8192C_PAGE_SIZE;
 132
 133         if (page_no > 8) {
 134                 RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
 135                          "Page numbers should not greater then 8\n");
 136         }
 137
 138         for (page = 0; page < page_no; page++) {
 139                 offset = page * FW_8192C_PAGE_SIZE;
 140                 _rtl88e_fw_page_write(hw, page, (buf_ptr + offset),
 141                                       FW_8192C_PAGE_SIZE);
 142         }
 143
 144         if (remain) {
 145                 offset = page_no * FW_8192C_PAGE_SIZE;
 146                 page = page_no;
 147                 _rtl88e_fw_page_write(hw, page, (buf_ptr + offset), remain);
 148         }
 149 }
 150
 151 static int _rtl88e_fw_free_to_go(struct ieee80211_hw *hw)
 152 {
 153         struct rtl_priv *rtlpriv = rtl_priv(hw);
 154         int err = -EIO;
 155         u32 counter = 0;
 156         u32 value32;
 157
 158         do {
 159                 value32 = rtl_read_dword(rtlpriv, REG_MCUFWDL);
 160         } while ((counter++ < FW_8192C_POLLING_TIMEOUT_COUNT) &&
 161                  (!(value32 & FWDL_CHKSUM_RPT)));
 162
 163         if (counter >= FW_8192C_POLLING_TIMEOUT_COUNT) {
 164                 RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
 165                          "chksum report faill ! REG_MCUFWDL:0x%08x .\n",
 166                           value32);
 167                 goto exit;
 168         }
 169
 170         RT_TRACE(rtlpriv, COMP_FW, DBG_TRACE,
 171                  "Checksum report OK ! REG_MCUFWDL:0x%08x .\n", value32);
 172
 173         value32 = rtl_read_dword(rtlpriv, REG_MCUFWDL);
 174         value32 |= MCUFWDL_RDY;
 175         value32 &= ~WINTINI_RDY;
 176         rtl_write_dword(rtlpriv, REG_MCUFWDL, value32);
 177
 178         rtl88e_firmware_selfreset(hw);
 179         counter = 0;
 180
 181         do {
 182                 value32 = rtl_read_dword(rtlpriv, REG_MCUFWDL);
 183                 if (value32 & WINTINI_RDY) {
 184                         RT_TRACE(rtlpriv, COMP_FW, DBG_TRACE,
 185                                  "Polling FW ready success!! REG_MCUFWDL:0x%08x.\n",
 186                                   value32);
 187                         err = 0;
 188                         goto exit;
 189                 }
 190
 191                 udelay(FW_8192C_POLLING_DELAY);
 192
 193         } while (counter++ < FW_8192C_POLLING_TIMEOUT_COUNT);
 194
 195         RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
 196                  "Polling FW ready fail!! REG_MCUFWDL:0x%08x .\n", value32);
 197
 198 exit:
 199         return err;
 200 }
 201
 202 int rtl88e_download_fw(struct ieee80211_hw *hw, bool buse_wake_on_wlan_fw)
 203 {
 204         struct rtl_priv *rtlpriv = rtl_priv(hw);
 205         struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
 206         struct rtl92c_firmware_header *pfwheader;
 207         u8 *pfwdata;
 208         u32 fwsize;
 209         int err;
 210         enum version_8188e version = rtlhal->version;
 211
 212         if (!rtlhal->pfirmware)
 213                 return 1;
 214
 215         pfwheader = (struct rtl92c_firmware_header *)rtlhal->pfirmware;
 216         pfwdata = (u8 *)rtlhal->pfirmware;
 217         fwsize = rtlhal->fwsize;
 218         RT_TRACE(rtlpriv, COMP_FW, DBG_DMESG,
 219                  "normal Firmware SIZE %d\n", fwsize);
 220
 221         if (IS_FW_HEADER_EXIST(pfwheader)) {
 222                 RT_TRACE(rtlpriv, COMP_FW, DBG_DMESG,
 223                          "Firmware Version(%d), Signature(%#x), Size(%d)\n",
 224                          pfwheader->version, pfwheader->signature,
 225                          (int)sizeof(struct rtl92c_firmware_header));
 226
 227                 pfwdata = pfwdata + sizeof(struct rtl92c_firmware_header);
 228                 fwsize = fwsize - sizeof(struct rtl92c_firmware_header);
 229         }
 230
 231         if (rtl_read_byte(rtlpriv, REG_MCUFWDL) & BIT(7)) {
 232                 rtl_write_byte(rtlpriv, REG_MCUFWDL, 0);
 233                 rtl88e_firmware_selfreset(hw);
 234         }
 235         _rtl88e_enable_fw_download(hw, true);
 236         _rtl88e_write_fw(hw, version, pfwdata, fwsize);
 237         _rtl88e_enable_fw_download(hw, false);
 238
 239         err = _rtl88e_fw_free_to_go(hw);
 240
 241         RT_TRACE(rtlpriv, COMP_FW, DBG_DMESG,
 242                  "Firmware is%s ready to run!\n", err ? " not" : "");
 243         return 0;
 244 }
 245
 246 static bool _rtl88e_check_fw_read_last_h2c(struct ieee80211_hw *hw, u8 boxnum)
 247 {
 248         struct rtl_priv *rtlpriv = rtl_priv(hw);
 249         u8 val_hmetfr;
 250
 251         val_hmetfr = rtl_read_byte(rtlpriv, REG_HMETFR);
 252         if (((val_hmetfr >> boxnum) & BIT(0)) == 0)
 253                 return true;
 254         return false;
 255 }
 256
 257 static void _rtl88e_fill_h2c_command(struct ieee80211_hw *hw,
 258                                      u8 element_id, u32 cmd_len,
 259                                      u8 *cmd_b)
 260 {
 261         struct rtl_priv *rtlpriv = rtl_priv(hw);
 262         struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
 263         u8 boxnum;
 264         u16 box_reg = 0, box_extreg = 0;
 265         u8 u1b_tmp;
 266         bool isfw_read = false;
 267         u8 buf_index = 0;
 268         bool write_sucess = false;
 269         u8 wait_h2c_limit = 100;
 270         u8 wait_writeh2c_limit = 100;
 271         u8 boxc[4], boxext[2];
 272         u32 h2c_waitcounter = 0;
 273         unsigned long flag;
 274         u8 idx;
 275
 276         RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD, "come in\n");
 277
 278         while (true) {
 279                 spin_lock_irqsave(&rtlpriv->locks.h2c_lock, flag);
 280                 if (rtlhal->h2c_setinprogress) {
 281                         RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
 282                                  "H2C set in progress! Wait to set..element_id(%d).\n",
 283                                  element_id);
 284
 285                         while (rtlhal->h2c_setinprogress) {
 286                                 spin_unlock_irqrestore(&rtlpriv->locks.h2c_lock,
 287                                                        flag);
 288                                 h2c_waitcounter++;
 289                                 RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
 290                                          "Wait 100 us (%d times)...\n",
 291                                          h2c_waitcounter);
 292                                 udelay(100);
 293
 294                                 if (h2c_waitcounter > 1000)
 295                                         return;
 296                                 spin_lock_irqsave(&rtlpriv->locks.h2c_lock,
 297                                                   flag);
 298                         }
 299                         spin_unlock_irqrestore(&rtlpriv->locks.h2c_lock, flag);
 300                 } else {
 301                         rtlhal->h2c_setinprogress = true;
 302                         spin_unlock_irqrestore(&rtlpriv->locks.h2c_lock, flag);
 303                         break;
 304                 }
 305         }
 306
 307         while (!write_sucess) {
 308                 wait_writeh2c_limit--;
 309                 if (wait_writeh2c_limit == 0) {
 310                         RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
 311                                  "Write H2C fail because no trigger for FW INT!\n");
 312                         break;
 313                 }
 314
 315                 boxnum = rtlhal->last_hmeboxnum;
 316                 switch (boxnum) {
 317                 case 0:
 318                         box_reg = REG_HMEBOX_0;
 319                         box_extreg = REG_HMEBOX_EXT_0;
 320                         break;
 321                 case 1:
 322                         box_reg = REG_HMEBOX_1;
 323                         box_extreg = REG_HMEBOX_EXT_1;
 324                         break;
 325                 case 2:
 326                         box_reg = REG_HMEBOX_2;
 327                         box_extreg = REG_HMEBOX_EXT_2;
 328                         break;
 329                 case 3:
 330                         box_reg = REG_HMEBOX_3;
 331                         box_extreg = REG_HMEBOX_EXT_3;
 332                         break;
 333                 default:
 334                         RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
 335                                  "switch case not processed\n");
 336                         break;
 337                 }
 338
 339                 isfw_read = _rtl88e_check_fw_read_last_h2c(hw, boxnum);
 340                 while (!isfw_read) {
 341                         wait_h2c_limit--;
 342                         if (wait_h2c_limit == 0) {
 343                                 RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
 344                                          "Wating too long for FW read "
 345                                          "clear HMEBox(%d)!\n", boxnum);
 346                                 break;
 347                         }
 348
 349                         udelay(10);
 350
 351                         isfw_read = _rtl88e_check_fw_read_last_h2c(hw, boxnum);
 352                         u1b_tmp = rtl_read_byte(rtlpriv, 0x130);
 353                         RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
 354                                  "Wating for FW read clear HMEBox(%d)!!! "
 355                                  "0x130 = %2x\n", boxnum, u1b_tmp);
 356                 }
 357
 358                 if (!isfw_read) {
 359                         RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
 360                                  "Write H2C register BOX[%d] fail!!!!! "
 361                                  "Fw do not read.\n", boxnum);
 362                         break;
 363                 }
 364
 365                 memset(boxc, 0, sizeof(boxc));
 366                 memset(boxext, 0, sizeof(boxext));
 367                 boxc[0] = element_id;
 368                 RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
 369                          "Write element_id box_reg(%4x) = %2x\n",
 370                          box_reg, element_id);
 371
 372                 switch (cmd_len) {
 373                 case 1:
 374                 case 2:
 375                 case 3:
 376                         /*boxc[0] &= ~(BIT(7));*/
 377                         memcpy((u8 *)(boxc) + 1, cmd_b + buf_index, cmd_len);
 378
 379                         for (idx = 0; idx < 4; idx++)
 380                                 rtl_write_byte(rtlpriv, box_reg+idx, boxc[idx]);
 381                         break;
 382                 case 4:
 383                 case 5:
 384                 case 6:
 385                 case 7:
 386                         /*boxc[0] |= (BIT(7));*/
 387                         memcpy((u8 *)(boxext), cmd_b + buf_index+3, cmd_len-3);
 388                         memcpy((u8 *)(boxc) + 1, cmd_b + buf_index, 3);
 389
 390                         for (idx = 0; idx < 2; idx++) {
 391                                 rtl_write_byte(rtlpriv, box_extreg + idx,
 392                                                boxext[idx]);
 393                         }
 394
 395                         for (idx = 0; idx < 4; idx++) {
 396                                 rtl_write_byte(rtlpriv, box_reg + idx,
 397                                                boxc[idx]);
 398                         }
 399                         break;
 400                 default:
 401                         RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
 402                                  "switch case not processed\n");
 403                         break;
 404                 }
 405
 406                 write_sucess = true;
 407
 408                 rtlhal->last_hmeboxnum = boxnum + 1;
 409                 if (rtlhal->last_hmeboxnum == 4)
 410                         rtlhal->last_hmeboxnum = 0;
 411
 412                 RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
 413                          "pHalData->last_hmeboxnum  = %d\n",
 414                          rtlhal->last_hmeboxnum);
 415         }
 416
 417         spin_lock_irqsave(&rtlpriv->locks.h2c_lock, flag);
 418         rtlhal->h2c_setinprogress = false;
 419         spin_unlock_irqrestore(&rtlpriv->locks.h2c_lock, flag);
 420
 421         RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD, "go out\n");
 422 }
 423
 424 void rtl88e_fill_h2c_cmd(struct ieee80211_hw *hw,
 425                          u8 element_id, u32 cmd_len, u8 *cmd_b)
 426 {
 427         struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
 428         u32 tmp_cmdbuf[2];
 429
 430         if (rtlhal->fw_ready == false) {
 431                 RT_ASSERT(false, "fail H2C cmd - Fw download fail!!!\n");
 432                 return;
 433         }
 434
 435         memset(tmp_cmdbuf, 0, 8);
 436         memcpy(tmp_cmdbuf, cmd_b, cmd_len);
 437         _rtl88e_fill_h2c_command(hw, element_id, cmd_len, (u8 *)&tmp_cmdbuf);
 438
 439         return;
 440 }
 441
 442 void rtl88e_firmware_selfreset(struct ieee80211_hw *hw)
 443 {
 444         u8 u1b_tmp;
 445         struct rtl_priv *rtlpriv = rtl_priv(hw);
 446
 447         u1b_tmp = rtl_read_byte(rtlpriv, REG_SYS_FUNC_EN+1);
 448         rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN+1, (u1b_tmp & (~BIT(2))));
 449         rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN+1, (u1b_tmp | BIT(2)));
 450         RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
 451                  "8051Reset88E(): 8051 reset success.\n");
 452 }
 453
 454 void rtl88e_set_fw_pwrmode_cmd(struct ieee80211_hw *hw, u8 mode)
 455 {
 456         struct rtl_priv *rtlpriv = rtl_priv(hw);
 457         u8 u1_h2c_set_pwrmode[H2C_88E_PWEMODE_LENGTH] = { 0 };
 458         struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
 459         u8 power_state = 0;
 460
 461         RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD, "FW LPS mode = %d\n", mode);
 462         SET_H2CCMD_PWRMODE_PARM_MODE(u1_h2c_set_pwrmode, ((mode) ? 1 : 0));
 463         SET_H2CCMD_PWRMODE_PARM_RLBM(u1_h2c_set_pwrmode, 0);
 464         SET_H2CCMD_PWRMODE_PARM_SMART_PS(u1_h2c_set_pwrmode,
 465                                          (rtlpriv->mac80211.p2p) ?
 466                                          ppsc->smart_ps : 1);
 467         SET_H2CCMD_PWRMODE_PARM_AWAKE_INTERVAL(u1_h2c_set_pwrmode,
 468                                                ppsc->reg_max_lps_awakeintvl);
 469         SET_H2CCMD_PWRMODE_PARM_ALL_QUEUE_UAPSD(u1_h2c_set_pwrmode, 0);
 470         if (mode == FW_PS_ACTIVE_MODE)
 471                 power_state |= FW_PWR_STATE_ACTIVE;
 472         else
 473                 power_state |= FW_PWR_STATE_RF_OFF;
 474         SET_H2CCMD_PWRMODE_PARM_PWR_STATE(u1_h2c_set_pwrmode, power_state);
 475
 476         RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_DMESG,
 477                       "rtl92c_set_fw_pwrmode(): u1_h2c_set_pwrmode\n",
 478                       u1_h2c_set_pwrmode, H2C_88E_PWEMODE_LENGTH);
 479         rtl88e_fill_h2c_cmd(hw, H2C_88E_SETPWRMODE, H2C_88E_PWEMODE_LENGTH,
 480                             u1_h2c_set_pwrmode);
 481 }
 482
 483 void rtl88e_set_fw_joinbss_report_cmd(struct ieee80211_hw *hw, u8 mstatus)
 484 {
 485         u8 u1_joinbssrpt_parm[1] = { 0 };
 486
 487         SET_H2CCMD_JOINBSSRPT_PARM_OPMODE(u1_joinbssrpt_parm, mstatus);
 488
 489         rtl88e_fill_h2c_cmd(hw, H2C_88E_JOINBSSRPT, 1, u1_joinbssrpt_parm);
 490 }
 491
 492 void rtl88e_set_fw_ap_off_load_cmd(struct ieee80211_hw *hw,
 493                                    u8 ap_offload_enable)
 494 {
 495         struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
 496         u8 u1_apoffload_parm[H2C_88E_AP_OFFLOAD_LENGTH] = { 0 };
 497
 498         SET_H2CCMD_AP_OFFLOAD_ON(u1_apoffload_parm, ap_offload_enable);
 499         SET_H2CCMD_AP_OFFLOAD_HIDDEN(u1_apoffload_parm, mac->hiddenssid);
 500         SET_H2CCMD_AP_OFFLOAD_DENYANY(u1_apoffload_parm, 0);
 501
 502         rtl88e_fill_h2c_cmd(hw, H2C_88E_AP_OFFLOAD, H2C_88E_AP_OFFLOAD_LENGTH,
 503                             u1_apoffload_parm);
 504 }
 505
 506 static bool _rtl88e_cmd_send_packet(struct ieee80211_hw *hw,
 507                                     struct sk_buff *skb)
 508 {
 509         struct rtl_priv *rtlpriv = rtl_priv(hw);
 510         struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
 511         struct rtl8192_tx_ring *ring;
 512         struct rtl_tx_desc *pdesc;
 513         struct sk_buff *pskb = NULL;
 514         unsigned long flags;
 515
 516         ring = &rtlpci->tx_ring[BEACON_QUEUE];
 517
 518         pskb = __skb_dequeue(&ring->queue);
 519         if (pskb)
 520                 kfree_skb(pskb);
 521
 522         spin_lock_irqsave(&rtlpriv->locks.irq_th_lock, flags);
 523
 524         pdesc = &ring->desc[0];
 525
 526         rtlpriv->cfg->ops->fill_tx_cmddesc(hw, (u8 *)pdesc, 1, 1, skb);
 527
 528         __skb_queue_tail(&ring->queue, skb);
 529
 530         spin_unlock_irqrestore(&rtlpriv->locks.irq_th_lock, flags);
 531
 532         rtlpriv->cfg->ops->tx_polling(hw, BEACON_QUEUE);
 533
 534         return true;
 535 }
 536
 537 #define BEACON_PG               0 /* ->1 */
 538 #define PSPOLL_PG               2
 539 #define NULL_PG                 3
 540 #define PROBERSP_PG             4 /* ->5 */
 541
 542 #define TOTAL_RESERVED_PKT_LEN  768
 543
 544 static u8 reserved_page_packet[TOTAL_RESERVED_PKT_LEN] = {
 545         /* page 0 beacon */
 546         0x80, 0x00, 0x00, 0x00, 0xFF, 0xFF, 0xFF, 0xFF,
 547         0xFF, 0xFF, 0x00, 0xE0, 0x4C, 0x76, 0x00, 0x42,
 548         0x00, 0x40, 0x10, 0x10, 0x00, 0x03, 0x50, 0x08,
 549         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 550         0x64, 0x00, 0x00, 0x04, 0x00, 0x0C, 0x6C, 0x69,
 551         0x6E, 0x6B, 0x73, 0x79, 0x73, 0x5F, 0x77, 0x6C,
 552         0x61, 0x6E, 0x01, 0x04, 0x82, 0x84, 0x8B, 0x96,
 553         0x03, 0x01, 0x01, 0x06, 0x02, 0x00, 0x00, 0x2A,
 554         0x01, 0x00, 0x32, 0x08, 0x24, 0x30, 0x48, 0x6C,
 555         0x0C, 0x12, 0x18, 0x60, 0x2D, 0x1A, 0x6C, 0x18,
 556         0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 557         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 558         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 559         0x3D, 0x00, 0xDD, 0x06, 0x00, 0xE0, 0x4C, 0x02,
 560         0x01, 0x70, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 561         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 562
 563         /* page 1 beacon */
 564         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 565         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 566         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 567         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 568         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 569         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 570         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 571         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 572         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 573         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 574         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 575         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 576         0x10, 0x00, 0x20, 0x8C, 0x00, 0x12, 0x10, 0x00,
 577         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 578         0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 579         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 580
 581         /* page 2  ps-poll */
 582         0xA4, 0x10, 0x01, 0xC0, 0x00, 0x40, 0x10, 0x10,
 583         0x00, 0x03, 0x00, 0xE0, 0x4C, 0x76, 0x00, 0x42,
 584         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 585         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 586         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 587         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 588         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 589         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 590         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 591         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 592         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 593         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 594         0x18, 0x00, 0x20, 0x8C, 0x00, 0x12, 0x00, 0x00,
 595         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x80,
 596         0x80, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 597         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 598
 599         /* page 3  null */
 600         0x48, 0x01, 0x00, 0x00, 0x00, 0x40, 0x10, 0x10,
 601         0x00, 0x03, 0x00, 0xE0, 0x4C, 0x76, 0x00, 0x42,
 602         0x00, 0x40, 0x10, 0x10, 0x00, 0x03, 0x00, 0x00,
 603         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 604         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 605         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 606         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 607         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 608         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 609         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 610         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 611         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 612         0x72, 0x00, 0x20, 0x8C, 0x00, 0x12, 0x00, 0x00,
 613         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x80,
 614         0x80, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 615         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 616
 617         /* page 4  probe_resp */
 618         0x50, 0x00, 0x00, 0x00, 0x00, 0x40, 0x10, 0x10,
 619         0x00, 0x03, 0x00, 0xE0, 0x4C, 0x76, 0x00, 0x42,
 620         0x00, 0x40, 0x10, 0x10, 0x00, 0x03, 0x00, 0x00,
 621         0x9E, 0x46, 0x15, 0x32, 0x27, 0xF2, 0x2D, 0x00,
 622         0x64, 0x00, 0x00, 0x04, 0x00, 0x0C, 0x6C, 0x69,
 623         0x6E, 0x6B, 0x73, 0x79, 0x73, 0x5F, 0x77, 0x6C,
 624         0x61, 0x6E, 0x01, 0x04, 0x82, 0x84, 0x8B, 0x96,
 625         0x03, 0x01, 0x01, 0x06, 0x02, 0x00, 0x00, 0x2A,
 626         0x01, 0x00, 0x32, 0x08, 0x24, 0x30, 0x48, 0x6C,
 627         0x0C, 0x12, 0x18, 0x60, 0x2D, 0x1A, 0x6C, 0x18,
 628         0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 629         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 630         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 631         0x3D, 0x00, 0xDD, 0x06, 0x00, 0xE0, 0x4C, 0x02,
 632         0x01, 0x70, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 633         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 634
 635         /* page 5  probe_resp */
 636         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 637         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 638         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 639         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 640         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 641         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 642         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 643         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 644         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 645         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 646         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 647         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 648         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 649         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 650         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 651         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 652 };
 653
 654 void rtl88e_set_fw_rsvdpagepkt(struct ieee80211_hw *hw, bool b_dl_finished)
 655 {
 656         struct rtl_priv *rtlpriv = rtl_priv(hw);
 657         struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
 658         struct sk_buff *skb = NULL;
 659
 660         u32 totalpacketlen;
 661         u8 u1RsvdPageLoc[5] = { 0 };
 662
 663         u8 *beacon;
 664         u8 *pspoll;
 665         u8 *nullfunc;
 666         u8 *probersp;
 667         /*---------------------------------------------------------
 668          *                      (1) beacon
 669          *---------------------------------------------------------
 670          */
 671         beacon = &reserved_page_packet[BEACON_PG * 128];
 672         SET_80211_HDR_ADDRESS2(beacon, mac->mac_addr);
 673         SET_80211_HDR_ADDRESS3(beacon, mac->bssid);
 674
 675         /*-------------------------------------------------------
 676          *                      (2) ps-poll
 677          *--------------------------------------------------------
 678          */
 679         pspoll = &reserved_page_packet[PSPOLL_PG * 128];
 680         SET_80211_PS_POLL_AID(pspoll, (mac->assoc_id | 0xc000));
 681         SET_80211_PS_POLL_BSSID(pspoll, mac->bssid);
 682         SET_80211_PS_POLL_TA(pspoll, mac->mac_addr);
 683
 684         SET_H2CCMD_RSVDPAGE_LOC_PSPOLL(u1RsvdPageLoc, PSPOLL_PG);
 685
 686         /*--------------------------------------------------------
 687          *                      (3) null data
 688          *---------------------------------------------------------
 689          */
 690         nullfunc = &reserved_page_packet[NULL_PG * 128];
 691         SET_80211_HDR_ADDRESS1(nullfunc, mac->bssid);
 692         SET_80211_HDR_ADDRESS2(nullfunc, mac->mac_addr);
 693         SET_80211_HDR_ADDRESS3(nullfunc, mac->bssid);
 694
 695         SET_H2CCMD_RSVDPAGE_LOC_NULL_DATA(u1RsvdPageLoc, NULL_PG);
 696
 697         /*---------------------------------------------------------
 698          *                      (4) probe response
 699          *----------------------------------------------------------
 700          */
 701         probersp = &reserved_page_packet[PROBERSP_PG * 128];
 702         SET_80211_HDR_ADDRESS1(probersp, mac->bssid);
 703         SET_80211_HDR_ADDRESS2(probersp, mac->mac_addr);
 704         SET_80211_HDR_ADDRESS3(probersp, mac->bssid);
 705
 706         SET_H2CCMD_RSVDPAGE_LOC_PROBE_RSP(u1RsvdPageLoc, PROBERSP_PG);
 707
 708         totalpacketlen = TOTAL_RESERVED_PKT_LEN;
 709
 710         RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_LOUD,
 711                       "rtl88e_set_fw_rsvdpagepkt(): HW_VAR_SET_TX_CMD: ALL\n",
 712                       &reserved_page_packet[0], totalpacketlen);
 713         RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_DMESG,
 714                       "rtl88e_set_fw_rsvdpagepkt(): HW_VAR_SET_TX_CMD: ALL\n",
 715                       u1RsvdPageLoc, 3);
 716
 717         skb = dev_alloc_skb(totalpacketlen);
 718         if (!skb)
 719                 return;
 720         kmemleak_not_leak(skb);
 721         memcpy(skb_put(skb, totalpacketlen),
 722                &reserved_page_packet, totalpacketlen);
 723
 724         if (_rtl88e_cmd_send_packet(hw, skb)) {
 725                 RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD,
 726                          "Set RSVD page location to Fw.\n");
 727                 RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_DMESG,
 728                               "H2C_RSVDPAGE:\n", u1RsvdPageLoc, 3);
 729                 rtl88e_fill_h2c_cmd(hw, H2C_88E_RSVDPAGE,
 730                                     sizeof(u1RsvdPageLoc), u1RsvdPageLoc);
 731         } else
 732                 RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
 733                          "Set RSVD page location to Fw FAIL!!!!!!.\n");
 734 }
 735
 736 /*Shoud check FW support p2p or not.*/
 737 static void rtl88e_set_p2p_ctw_period_cmd(struct ieee80211_hw *hw, u8 ctwindow)
 738 {
 739         u8 u1_ctwindow_period[1] = {ctwindow};
 740
 741         rtl88e_fill_h2c_cmd(hw, H2C_88E_P2P_PS_CTW_CMD, 1, u1_ctwindow_period);
 742 }
 743
 744 void rtl88e_set_p2p_ps_offload_cmd(struct ieee80211_hw *hw, u8 p2p_ps_state)
 745 {
 746         struct rtl_priv *rtlpriv = rtl_priv(hw);
 747         struct rtl_ps_ctl *rtlps = rtl_psc(rtl_priv(hw));
 748         struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
 749         struct rtl_p2p_ps_info *p2pinfo = &(rtlps->p2p_ps_info);
 750         struct p2p_ps_offload_t *p2p_ps_offload = &rtlhal->p2p_ps_offload;
 751         u8      i;
 752         u16     ctwindow;
 753         u32     start_time, tsf_low;
 754
 755         switch (p2p_ps_state) {
 756         case P2P_PS_DISABLE:
 757                 RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "P2P_PS_DISABLE\n");
 758                 memset(p2p_ps_offload, 0, sizeof(struct p2p_ps_offload_t));
 759                 break;
 760         case P2P_PS_ENABLE:
 761                 RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "P2P_PS_ENABLE\n");
 762                 /* update CTWindow value. */
 763                 if (p2pinfo->ctwindow > 0) {
 764                         p2p_ps_offload->ctwindow_en = 1;
 765                         ctwindow = p2pinfo->ctwindow;
 766                         rtl88e_set_p2p_ctw_period_cmd(hw, ctwindow);
 767                 }
 768                 /* hw only support 2 set of NoA */
 769                 for (i = 0; i < p2pinfo->noa_num; i++) {
 770                         /* To control the register setting for which NOA*/
 771                         rtl_write_byte(rtlpriv, 0x5cf, (i << 4));
 772                         if (i == 0)
 773                                 p2p_ps_offload->noa0_en = 1;
 774                         else
 775                                 p2p_ps_offload->noa1_en = 1;
 776
 777                         /* config P2P NoA Descriptor Register */
 778                         rtl_write_dword(rtlpriv, 0x5E0,
 779                                         p2pinfo->noa_duration[i]);
 780                         rtl_write_dword(rtlpriv, 0x5E4,
 781                                         p2pinfo->noa_interval[i]);
 782
 783                         /*Get Current TSF value */
 784                         tsf_low = rtl_read_dword(rtlpriv, REG_TSFTR);
 785
 786                         start_time = p2pinfo->noa_start_time[i];
 787                         if (p2pinfo->noa_count_type[i] != 1) {
 788                                 while (start_time <= (tsf_low + (50 * 1024))) {
 789                                         start_time += p2pinfo->noa_interval[i];
 790                                         if (p2pinfo->noa_count_type[i] != 255)
 791                                                 p2pinfo->noa_count_type[i]--;
 792                                 }
 793                         }
 794                         rtl_write_dword(rtlpriv, 0x5E8, start_time);
 795                         rtl_write_dword(rtlpriv, 0x5EC,
 796                                         p2pinfo->noa_count_type[i]);
 797                 }
 798
 799                 if ((p2pinfo->opp_ps == 1) || (p2pinfo->noa_num > 0)) {
 800                         /* rst p2p circuit */
 801                         rtl_write_byte(rtlpriv, REG_DUAL_TSF_RST, BIT(4));
 802
 803                         p2p_ps_offload->offload_en = 1;
 804
 805                         if (P2P_ROLE_GO == rtlpriv->mac80211.p2p) {
 806                                 p2p_ps_offload->role = 1;
 807                                 p2p_ps_offload->allstasleep = 0;
 808                         } else {
 809                                 p2p_ps_offload->role = 0;
 810                         }
 811
 812                         p2p_ps_offload->discovery = 0;
 813                 }
 814                 break;
 815         case P2P_PS_SCAN:
 816                 RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "P2P_PS_SCAN\n");
 817                 p2p_ps_offload->discovery = 1;
 818                 break;
 819         case P2P_PS_SCAN_DONE:
 820                 RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "P2P_PS_SCAN_DONE\n");
 821                 p2p_ps_offload->discovery = 0;
 822                 p2pinfo->p2p_ps_state = P2P_PS_ENABLE;
 823                 break;
 824         default:
 825                 break;
 826         }
 827
 828         rtl88e_fill_h2c_cmd(hw, H2C_88E_P2P_PS_OFFLOAD, 1,
 829                             (u8 *)p2p_ps_offload);
 830 }