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